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ABSTRACT 

A path planning method with collision avoidance for a general single chain nonre- 
dundant or redundant robot is proposed. Joint range boundary overruns are also 
avoided. The result is a sequence of joint vectors which are passed to a trajectory 
planner. 

A potential field algorithm in joint space computes incremental joint vectors 
Aq = Aq a + Aq c + Aq r . Adding Aq to the robot’s current joint vector leads to 
the next step in the path. 

Aq a is obtained by computing the minimum norm solution of the underde- 
termined linear system JAq a = x a where x a is a translational and rotational force 
vector that attracts the robot to its goal position and orientation. J is the manipu- 
lator Jacobian. 

Aq c is a collision avoidance term encompassing collisions between the robot 
(links and payload) and obstacles in the environment as well as collisions among 
links and payload of the robot themselves. It is obtained in joint space directly. 

Aq r is a function of the current joint vector and avoids joint range overruns. 

A higher level discrete search over candidate safe positions is used to provide 
alternatives in case the potential field algorithm encounters a local minimum and 
thus fails to reach the goal. The best first search algorithm A* is used for graph 
search. Symmetry properties of the payload and equivalent rotations are exploited 
to further enlarge the number of alternatives passed to the potential field algorithm. ! 
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CHAPTER 1 
INTRODUCTION 


In earlier applications of robotic systems and in most present systems, the task of 
planning a feasible path is done by a human. The most basic way of entering the 
path is typing a sequence of points in cartesian or even in joint space and having the 
system perform a linear interpolation between the points. If the robot is operating 
in a constant environment, this approach is often sufficient. Some variation in the 
environment can be taken into account by prestoring a set of paths and selecting 
the appropriate one depending on the current state of the environment. 

In some applications, as for example spray painting a car part, a complex 
continuous motion must be accomplished. This kind of motion would be ver^ tedious 
to enter by typing long lists of cartesian or joint coordinates. In this case, a method 
known as “teach-in” is used, where a competent person is guiding the robot’s end 
effector, while the system is storing the motion. Later on, this motion can be 
reproduced repeatedly and accurately. 

The drawback of all these methods is their inability to work in a largely un- 
known environment. If an unexpected obstacle is entering the workspace, the robot 
will probably collide with it. 

In many applications it is desirable to have more intelligent robotic systems ca- 
pable of working more autonomously. The work herein has been done under CIRSSE 
(Center for Intelligent Robotic Systems for Space Exploration). The goal of CIRSSE 
is the development of planning, sensing, and control methods that will eventually 
allow mostly autonomous assembly of parts of a space station in outer space. Au- 
tonomy is particularly important in outer space, since risk and costs of assembly 
by humans are extremely high. Another application is autonomous exploration of 
other planets, where teleoperation is impossible due to large communication delays. 
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Figure 1.1: An example structure 


An example of a structure being assembled in space is the truss structure 
shown in Figure 1.1. The basic cell in this structure is the tetrahedron. Tetrahedra 
have two useful properties: They are mechanically stable and their six edges all have 
the same length, so only one type of strut is needed. Using them as a unit cell, one 
can build arbitrarily large, stable structures. 

The first step in doing this automatically is determining an assembly sequence 
such that subassemblies are stable whenever this is possible [6]. Once the sequence 
has been determined, a path planner is needed to find a collision free path to move 
the struts from their storage location to their goal position in the structure. This 
project is proposing a solution to the path planning problem. It is a free motion 
planner, so it does not cover the grasping and mating operations and the associated 
fine motion planning which requires an online visual feedback and force feedback. 
Another important part of the system is a high level supervisor which decides about 
new strategies if a part of the plan turns out to be unfeasible or if other unexpected 
events require a change of strategy. 
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The path planner is first requesting information about the current state of the 
environment. It needs to know the positions of all struts currently in the environ- 
ment and the current joint vector of the robot. After this initialization it accepts 
motion commands telling it to move the end effector to a certain position and ori- 
entation in cartesian space. It must also be informed of grasping and ungrasping 
operations, since the changes in the environment have an influence on the path plan- 
ning process. The path planner provides a sequence of joint vectors which is passed 
to the trajectory planner. In this stage the velocity and acceleration information is 
added and the result is passed to the low level robot controller. 

This text is organized as follows: Chapter 2 gives an overview of the different 
path planning strategies and algorithms that have been developed m the past. In 
chapter 3, the geometric model for object representation used in this work is in- 
troduced and a distance computation algorithm is presented. Chapter 4 covers the 
description of the robot used to execute the tasks and presents the local path plan- 
ning algorithm based on potential fields. The global path planning based on graph 
search is covered in chapter 5. The documentation of the path planner s implemen- 
tation is presented in chapter 6, and chapter 7 shows the results that have been 
obtained. A discussion of the results and conclusions can be found in chapter 8. An 
input file of a simulated task in included in appendix A and listings of the program’s 
header files can be found in appendix B. 

1.1 Nomenclature 

Throughout this text, scalars are printed in normal typeface (x or A'), vectors 
in lowercase bold face (x) and matrices in uppercase bold face letters (X). 


CHAPTER 2 
LITERATURE REVIEW 

Currently known path planning algorithms fall into two categories, global and local. 

The global methods typically consider the environment as a whole when plan- 
ning a path. They generally have the advantage of finding a path if one exists, but 
they are computationally expensive if the environment is complex. There are three 
subgroups of global path planning algorithms: 

• Cell decomposition methods 

• Search algorithms on a visibility graph 

• Optimization using calculus of variation 

Cell decomposition methods can be further divided into exact and approximate 
methods whereas exact methods decompose free space into cells complex enough to 
exactly represent the free space. Approximate methods however split free space 
into very simple units, for instance cubes, that cannot represent free space exactly. 
There will be a number of cubes that contain both free and occupied space. These 
cells are further subdivided into smaller cells, until the desired accuracy is obtained. 
After the cell model is established, graph search techniques are used to generate the 
shortest path that includes only cells representing free space [20], [21]. 

Instead of splitting the space into cells, one can also find a sufficiently large set 
of feasible intermediate goals, usually located near corners of obstacles, and establish 
a graph based on visibility between pairs of these goals. Both of these methods rely 
on an efficient graph search algorithm. A suitable algorithm is A*, a best first search 
algorithm that considers a lower bound estimate of the remaining distance in order 
to expand the most promising nodes first [4], [18]. 
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A different approach that doesn’t involve graph search is the optimization of a 
cost function that typically includes path length and clearance from obstacles. This 
approach uses methods from calculus of variation [19]. 

The oldest local path planning method is the “hypothesize and test” method. 
The robot proceeds a step in the desired direction and checks the feasibility of the 
path that leads to the new position. If the path is not feasible, another direction is 
chosen according to some heuristic. This approach has not been very successful. 

A more recent method is based on potential fields [7], [8], [9], [17]. The gradient 
of a potential field is guiding the robot towards the goal and keeps it at a safe 
distance from obstacles. This potential field has very large values near obstacles 
in order to produce a repulsive force between the robot and the obstacles. The 
potential assumes its lowest value at the goal position in order to attract the robot 
to the goal. The drawback of all potential field methods is the fact that they do 
not guarantee finding a collision free path even if one exists. If the potential field 
function is not carefully chosen, it is likely to contain local minima, in which the 
path planning algorithm can get stuck. An attempt to solve this problem is made 
in [10] by adding vortex fields. However, even if a path can be found, it is not 
guaranteed to be optimal. 

Variations of the potential field methods include information about the dy- 
namics of the robot and its payload. These methods have the advantage that they 
generally produce paths which can be executed at a comparatively high speed [15], 
[16]- 

If the robot is not assumed to be a point or a sphere, then its shape must be 
included in the path planning process. A particular path might be feasible depending 
on the orientation of the robot as a function of its position. In order to account for 
the robot’s orientation, the path planning problem can be solved in configuration 
space [22], [23], [24], [25]. The rotational degrees of freedom of the robot are added 
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as additional dimensions to the cartesian space. If we consider a three dimensional 
space and a robot with another three degrees of freedom for rotation, the associated 
configuration space is six dimensional. In configuration space, the robot is shrunk 
to a point and the obstacles are enlarged according to the robot’s orientation. This 
reduces the problem to planning a path for a point, however in a higher dimensional 


space. 


CHAPTER 3 

GEOMETRIC MODELING 

Every path planning algorithm needs a geometric description of the objects it works 
with. If a very simple model is used to represent the objects, it is in most cases 
necessary to choose the model’s volume considerably larger than the real object’s 
volume, since the object’s volume must be completely enclosed in the volume of its 
model. This restricts the path planning process and might lead to a failure even 
though a path exists. On the other hand, distance computations on simple models 
are typically performed very efficiently. More involved models allow for a better 
approximation to the real shape of the objects, but they are computationally more 
expensive. 

In this work, cylinders with spherical caps have been chosen as the basic unit 
for geometric modeling. An example is shown in Figure 3.1. It can also be viewed 
as the volume that is swept by a sphere with radius r moving between the two 
points pi and p 2 on a straight line. This volume can be described as a set S in the 
following way: 


S = P2 - Pi 

S(Pi,P2,r) = {p|p = pi+*s + q with t € [0 — 1], |q| < r} (3.1) 

This model is particularly well suited for the objects of interest in strut-based 
structure assembly, namely struts and robot links, but any kind of object can be 
approximated by using a union of several basic cylinders. 

This chapter presents the distance computation for this particular model in 
detail, but every other model as for instance a polyhedral model can be used, as 
long as the two points of closest distance between two objects can be computed. 
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Figure 3.1: Basic geometric model 


A generalization of the swept sphere model discussed here is a swept sphere model 
in which the sphere radius is a linear function of the sphere’s position between the 
endpoints: The radius is r = (1— t)r 1 +tr 2 where r-j is the radius of the sphere at point 
pi and r 2 the radius at point p 2 - This model allows for more accurate representation 
of conic objects and also provides a very efficient distance computation method [26], 
In the following the distance computation algorithm for the cylindrical model 
is presented (Figure 3.2). In order to find the distance d between two objects, we 
find the distance d s between the two line segments in the center of the cylinders and 
subtract the radii of the cylinders r p and r 9 . 





d = d,-r p -r q (3.2) 

The line segment in the center of a cylinder S(pi,p 2 ,r) can be described as 

S(Pl,P2,0). 

The remainder of this chapter discusses the computation of d a , the distance 
between the line segments of two cylinders. 



Figure 3.2: Shortest distance between two swept sphere cylinders 

The distance between two line segments S p (pi,p2,0) and S,(qi, q2, 0) can be 
obtained by first finding the distance between the two unbounded lines defined by 
the endpoints of the line segments. The two unbounded lines can be described as 
follows (see also Figure 3.3): 


Sp = P2 ~ Pi 

Sq = q2 — qi 

p = Pi + t p Sp (3.3) 

q = qi+<?s q (3.4) 

The parameters t v and are real numbers. If the lines are not parallel, then 
the two points of minimal distance 


Pmin 

— Pi “1" tpminSp 

(3.5) 

Qmin 

— ■ Ql *4* igmtnSq 

(3.6) 


10 




Figure 3.3: Shortest distance between two line segments 


can be obtained by solving the following linear system for i pmin and t 


qmm • 


Pl ^pmtn® p “I” ^(Sp •*' Sq) CJl "b ^gmin®q 


(3.7) 


If these points are also on the line segments (t pm i n € [0 ... 1] and t qm in € 
[0 ... 1]), then the distance |p m j n — qminl is the distance between the line segments 
and the desired distance d between the cylinders is: 


d — |Pmin 9Lmin| ~ T p T q (3-8) 

If t p or t q or both are not element of [0... 1] as it is the case in Figure 3.3, 
then we define an auxiliary distance d p as follows: 


IPi * Pminl for ^ < 0 
|P2 - Pminl for tp > 1 
0 otherwise 


(3.9) 
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Figure 3.4: Distance computation 

An equivalent distance d q for the other line is also defined. The values d p and 
d q represent the distances between the points p m jn Qmin an d their line segments 
S p ( pi, P 2 , 0) and S,(qi, q 2 , 0) respectively. Let us also define the two points z p and 
Zq of minimal distance on the two line segments: 

min |z p — z q | subject to Zp € S p (pi, P2,0) and z q € 5,(qj, q 2 , 0) (3.10) 

For the remainder of the discussion we can assume without loss of generality 
that d p > d q , t p < 0 and t q < 0. Consider Figure 3.4 which shows a projection of the 
two line segments on a plane perpendicular to the line defined by p m in an< ^ qmin- 
Let us also assume that angle a < 90°. 

Under these assumptions we can state that z p = pi. 
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Proof: Consider the orthogonal coordinate system (u,v) in Figure 3.4. Let us first 
state that the v coordinate of qi is always negative, since d p > d q . We can also 
state that if z p ^ px then the v coordinate of z q must be positive and equal to the 
v coordinate of z p (not considering the case that z p = P 2 ). If the v coordinate of 
q 2 is negative, then a z q with a positive v coordinate does not exist. Otherwise z q 
must be an element of S' = S(y, q 2 , 0)->y. y is the element of S(qi,q2,0) with a 
zero v coordinate. But since S' is an open set, there is a point z q ' € S' in every 
neighborhood of z q such that |z p — z q '| < |z p — z q |, so z p ^ pi does not exist. □ 
This reduces the problem to finding the minimum distance between point pi 
and line segment 5(qi,q2,0). The point qmi n on the unbounded line defined in 
equation 3.4 closest to pi is defined by: 


Qmin — Ql 4" lgim'n®q 

4 _ Sq-(pi-qi) 

Lqmin — "" ‘ 

S q • S q 

From this we can find z q as follows: 


(3.11) 



qi 

for tq m i n < 0 



Zq — i 

Q2 

for tqmin ^ 1 


(3.12) 


Qmin 

otherwise 




Then d 3 = |z p — z q | and we can find the distance between the cylinders 
using equation 3.2. This concludes the discussion of the distance computation for 
nonparallel line segments. 

If the line segments are parallel, then we compute the four distances 

di : distance between pi andS,(qx, q 2 , 0) 
dj : distance between P 2 andS 9 (qx, q 2 , 0) 
d 3 : distance between qx andS p (pi, P 2 , 0) 
d 4 : distance between q 2 andS p (pi,p2,0) 
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using the algorithm described above. It turns out that at least two of the four 
distances will be equal to the minimum distance d 5 , so it is sufficient to compute any 
three of the four distances and find the minimum, for instance d s = min (d u d 2 , d 3 ). 


CHAPTER 4 

USING A ROBOT TO FOLLOW A PATH 


In real applications, objects are moved from their start to their goal positions by a 
robot, so the robot’s geometry must be taken into consideration. The robot imposes 
constraints on the path in that the path must entirely lie in the robot’s workspace. 
It also adds to the list of parts that may be involved in a collision. There may be 
collisions between obstacles and parts of the robot, and there may even be collisions 
among different parts of the robot itself. The limited range of the robot’s joints 
further complicate things. 

In this work, a single robot that holds a cylindrical object at its center is 
considered. The method presented could be used for all single chain robots with at 
least six degrees of freedom. All joints can be either revolute or prismatic. 

4.1 Modeling the robot’s geometry 

In the process of path planning, we must consider collisions between different 
struts, collisions between a strut and a part of the robot and collisions among parts 
of the robot. In order to simplify distance computations, it makes sense to use the 
same model for robot links and other parts. The cylindrical model as shown in 
Figure 3.1 can also provide sufficient modeling accuracy for the robot links when 
performing free motion planning. Distance computation is done as described in 
chapter 3. 

4.2 Potential field method in the robot’s joint space 

Potential field methods as outlined in chapter 2 work in cartesian space. All 
gradients of the potential fields are vectors in normal three dimensional space. They 
define the direction of motion of the robot’s end effector, so the new location is 
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first given in cartesian space. This means that an inverse kinematics routine is 
required to find the new joint vector. Since this must be done at every step of the 
path, the algorithm becomes very computationally expensive, since in general the 
inverse kinematics problem can only be solved iteratively. In this work, however, 
an alternate strategy is taken whereby the robot’s new location is first obtained in 
joint space, rather than in cartesian space. To find the location in cartesian space, 
a simple forward kinematics routine is required. 

Thus the goal of the algorithm is to find a joint vector increment Aq that is 
added to the current joint vector q to obtain the next step of the path. Aq is the 
sum of several components each performing a different task: 

Aq = Aq a + Aq r (4.1) 

Aq a is the joint vector increment that moves the payload closer to its goal 
position and orientation (attractive forces), while Aq r is the sum of all joint vector 
increments that keep the robot from colliding or running out of joint range (repulsive 
forces). 


Aq r = Aqrc + Aq^ (4.2) 

where Aq rc is doing the collision avoidance and Aq rr keeps the robot from 
running out of joint range. 

Note that all fields are defined as gradient fields and not as potential fields. 
This often leads to a more natural definition and also saves the derivation of the 
field’s gradient. 

4.2.1 Attraction Field 

The attractive field is the only field that must be defined in cartesian space. 
First the definition of the cartesian vector x a is given, then the transformation into 
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joint space is shown. 

x a is a 6 by 1 vector. The first three elements (x a t) represent the translational 
part (force) while the last three elements (Xar) represent the rotational part (torque): 



(4.3) 


If p c is the cartesian position of the center of the strut in the robot’s gripper 
(current position) and g c the cartesian position of the center of the strut in its goal 
position, then 


Xat = C at | gc - Pc , (4.4) 

l8c-Pe| 

C at is the field constant of the translational attraction field. The rotational 
part Xar is more difficult to obtain. Let pi and p 2 be the endpoints of the object 
in the robot’s gripper and let gi and g 2 be the endpoints of the same object in its 
goal position. Furthermore let a p be the approach vector of the end effector in its 
current orientation and let a g be the approach vector of the end effector when it 
reaches its goal orientation. The approach vector represents the direction the end 
effector must move to properly approach an object before grasping it. Both a p and 
a g are unit vectors. We also need two unit vectors that point along the axes of the 
struts: 

_ P2 ~ P* 

P |P2 — Pll 

_ g2 ~ gl 

Pg Ig2 — gl I ■ 7 ^ -- 

The approach vectors are perpendicular to the axes of the struts: 


a p 

a g 


X p p 

-L Pg 


1 


(4.5) 

(4.6) 
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( 4 . 9 ) 
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This is the rotation we are interested in, but we need the axis of rotation and 
not the rotation matrix. In order to find the axis of rotation, an eigenvalue problem 
must be solved. Every rotation matrix has an eigenvalue of 1 and a corresponding 
eigenvector a. The axis of the rotation performed by the rotation matrix and eigen- 
vector a have the same orientation. Vector a can be found by solving the following 
linear system: 


(T — I)a = 0 (4.10) 

This system can be solved by first setting the first component of a to 1. If 
solving for the other two components fails, then we set the second component to 1 
and try to solve for the other two components. If this also fails, we set the third 
component to 1 and solve for the first two. 

The vector a represents the axis of rotation, but it doesn’t give any information 
about the sense of rotation, since —a is a valid eigenvector if a is an eigenvector. 
We can of course rotate both ways to reach the goal orientation but in most cases it 
is preferable to rotate in the sense that involves a smaller angle of rotation. For the 
method used to find the correct sense of rotation or the correct sign of a consider 
Figure 4.2. 

It is assumed that p can be rotated into g by using a as the axis of rotation. 
The angle of rotation is less than or equal to 180° if the following condition holds: 

g-(axp)>0 (4.11) 

If the condition doesn’t hold, we just change the sign of a to get the correct 
sense of rotation. Then the rotational part from equation 4.3 is: 


Xar - Car** 


(4.12) 


hi ill ill u< 
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Figure 4.2: Sense of rotation 

C aT is again a field constant. All this yields x a , a direction vector in cartesian 
space. In order to avoid inverse kinematics, a direction vector in joint space is 
needed. The direction vectors in either space can be considered velocity vectors, 
so the manipulator Jacobian J can be used to transform the direction vector from 
cartesian to joint space. The derivation of J is discussed in subsection 4.2.3, so let 
us assume for now that it is available to us. In order to obtain Aq a from equation 
4.1 we need to solve the following linear system: 

JAq a = x a (4.13) 

The Jacobian J is a 6 by n matrix, where n is the number of degrees of freedom 
of the manipulator. If n > 6 then the linear system is underdetermined, so there is 
an infinite number of solutions for A<3a- In this case the term Aq^QAq a will be 
minimized, where Q is a weight matrix that will normally have nonzero terms on 
the diagonal only. This weight matrix is important for robots with both revolute 
and prismatic joints, since vector q a has mixed units in this case. A method for 
solving this minimization problem is presented in subsection 4.2.6. 

This method is a variation of iterative inverse kinematics algorithms that have 
been developed for redundant robots or for nonredundant robots with kinematic 
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properties for which no closed form inverse kinematics solution exists. [1], [2], [3], 
[11], [12], [13], [14]. 

4.2.2 Obstacle Avoidance 

The goal of this part of the algorithm is to keep the moving objects (the links 
and the payload of the robot) from colliding with items in the environment and 
also to avoid collisions among the moving parts. These goals are achieved by two 
separate algorithms that both contribute a joint vector to Aq rc from equation 4.2. 

Aq rc — Aqenv d" Aqself (4-14) 

Before beginning the discussion of the two methods, it is worth mentioning that 
the moving parts are the manipulator links and the object held by the manipulator. 
The payload is treated exactly like a manipulator link. 

The idea behind both methods for collision avoidance is to go through all pairs 
of items that may collide and for each pair go through all joints that may change 
the distance between the items of the pair. The joints that have an influence on 
the distance of two objects are located between the objects in the kinematic chain. 
Adding an increment to these joint values may increase or decrease the distance or it 
may have no effect on the distance at all. Figure 4.3 shows two items modeled using 
the cylindrical model presented in chapter 3. The joint in Figure 4.3 is a revolute 
joint; prismatic joints are discussed later. 

The points z p and z q are the two closest possible points on the line segments of 
the models, they are found using the algorithm described in chapter 3. The distance 
d is the minimum distance between the models (see equation 3.2). The vectors a Q 
and a<j represent the axis of rotation of the joint under consideration. a<> is the 
origin of the frame rotated by that joint and a^ is a unit vector representing the 
direction of the axis of rotation. Now we define a unit vector r pointing from z p 
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Figure 4.3: Basic collision avoidance procedure for a revolute joint 
towards z q : 

r = , Zq (4.15) 

We assume that model S q is moving and model 5 P is fixed, then vector r is the 
best possible direction in which model s q and point z q in particular can be moved 
in order to avoid a collision between the two models. We also define a vector s as 
follows: 

s = a d x(z q -a 0 ) (4.16) 

Vector s represents the direction in which point z q will move when the joint 
is rotated in positive sense of rotation. Vector s is really this joint’s column of the 
Jacobian matrix for point z q . Note that the absolute value of s is equal to the 
distance between z q and the axis of rotation. Now we can define the joint increment 
6q T that will be added to the component of this joint in the joint vector increment: 
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Sq T =C r - ^ (4.17) 

It turns out that a degree of repulsion proportional to d ~ 2 leads to good results. 
The repulsive force grows very fast when the models get close together. When the 
distance gets larger, the force goes down fast enough, so that there is almost no 
influence on the path when the distances to obstacles are reasonably large. The 
force is also proportional to |s| increasing the contribution of joints that have their 
axis of rotation far off the point of interest. These joints can cause a large increase in 
distance between the objects with comparatively small angles of rotation. The force 
is finally proportional to the dot product of r and s which is a degree of matching 
between the optimal direction to increase the critical distance and the direction that 
Zq will actually go if the current joint is moved. Note that this dot product can be 
negative, which means that the joint must be rotated in negative sense of rotation 
in order to increase the critical distance. 

Figure 4.4 shows the same situation with a prismatic joint. 

The variables Zp, Zq, r , slq, and d are defined as in the case of a revolute 
joint. The vector au is the direction of motion of this joint, so 8q v is defined as 
follows: 


% = C/-^ (4-18) 

Based on this elementary procedure we can define the joint vector increments 
q env and qself from equation 4.14. For this purpose we define a function fq(-,*,-) 
as follows: 


f q (A, B,i) = [ui ...v n ] T where n is the number of joints (4.19) 
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Figure 4.4: Basic collision avoidance procedure for a prismatic joint 


Vj = 

The points z p and z q and the distance d are found by applying the distance 
computation algorithm described in chapter 3 to the models A and B. 

Furthermore, let 0„ i € [1 ... o] be the model of the *th obstacle in the envi- 
ronment and L h j € [1 . • • 1} the model of the jth link of the robot. Thus there are 
o obstacles and / links to be considered. Then q e nv and q^lf are defined as follows. 

qenv = Lj,k) 

1 = 1 ]—\ fc= 1 

and 

CUelf = EE t 


6q T for j = i and joint i revolute 
6q p for j = i and joint i prismatic 
0 for j ^ i 


(4.20) 


(4.22) 
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Figure 4.5: Numbering convention for the joints and links of the robot 

The numbering convention of the joints and links is shown in Figure 4.5. Note 
that the robot’s base must be modeled as an obstacle and not as a link. 

This concludes the derivation of Aq rc . 


1 


4.2.3 Derivation of the manipulator Jacobian 


Before proceeding to the joint range problem, let us discuss the derivation 
of the manipulator Jacobian J. The origins of the link frames (a 0 ),- and the joint 
axes (aa); have been introduced in the previous subsection, as they are used for 
collision avoidance. Once they have been computed, the manipulator Jacobian can 
be obtained very easily and with relatively little computational effort: 



J2 



(4.23) 


with n being the number of degrees of freedom of the manipulator. Then 



(ad), x (P ~ (ao)i) 
(ad). 


(4.24) 


■ ! 



if joint i is a revolute joint or 
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Figure 4.6: 6q r {q) 


ji = 


(ad). 

0 


(4.25) 


if joint i is a prismatic joint; vector p is the current end effector position. The 
computation of a row corresponding to a revolute joint involves a cross product and 
a vector subtraction, while the rows corresponding to prismatic joints are already 

fully computed. 


4.2.4 Joint Range 

Besides avoiding collisions, a path planner must also guarantee that every 
joint vector on the path is in the robot’s joint range. This goal can be achieved by 
monitoring the joint angles and creating a corrective joint value 5q R if an angle is 
close to an end of its range. These values for all joints form the joint vector Aq^ 
from equation 4.2. The value 6q R is a function of the respective joint value q and 
should behave similarly to the function shown in Figure 4.6. 


u 
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Figure 4.7: Domain mapping for joint range function 

In our current implementation, the function Sq R is defined as follows: 

The domain of q , [q m in • • • ?max]i is first mapped onto the interval [— 1 . . . l] by 
applying the function 


u(q) = ~ — — with q = 3sisi±3sis. an d R q = q max - q min (4.26) 

Kq/2 

A graph of this mapping is shown in Figure 4.7. 

On the domain u, a function r(u ) as shown in Figure 4.8 is defined: 


a 

i 


r(u) = 


u + b 


+ c 


This function is used to define 6qn(u ) as follows: 


(4.27) 


Sq r (u) = < 


-C R r( 1 - u) 


for u > 0 


(4.28) 


( C R r(u -f 1) for u < 0 

The parameters a, b and c in equation 4.27 determine the shape of function 
r(u), but their geometrical influence on the function is not intuitively clear. For this 
reason we introduce the parameters Zt and j/o a ls° shown in Figure 4.8. From the 
three equations 


yo = r(o) 


(4.29) 
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0 xl 1 

Figure 4.8: r(u) 


(4.30) 

(4.31) 


1 = r(x x ) 

0 = r(l) 

we can find the expressions for a, b and c in terms of X\ and j/o ; 


Xi2/o 

c = 

1 — Vo + x iVo 

b = — — 

yo 

a = — c(b + 1) 


(4.32) 

(4.33) 

(4.34) 


The values for X! and y 0 must be found by trial and error. It was determined 
that xi = 0.2 and y 0 = 10 lead to satisfactory results. 


4.2.5 Imposing restrictions in cartesian space 

Recall the attraction field described in subsection 4.2.1. Translation and ro- 
tation to the goal position is achieved by finding a direction vector and an axis of 
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rotation that describe the necessary motion (equation 4.3). Then we solve for the 
joint vector using the manipulator Jacobian (equation 4.13). The two algorithms 
that provide direction vector and axis of rotation are independent and the length of 
the direction vector as well as angle of rotation are constant for each step. A con- 
sequence of this approach is the fact that rotation and translation are generally not 
completed at the same time. In a given situation it might happen that the payload 
has reached the correct orientation, but it has not reached the goal yet. In this case 
we would detect that the correct orientation has been reached and thus set x ar in 
equation 4.3 to zero. After solving equation 4.13 we obtain a joint vector increment 
that will not rotate the end effector when it is added to the current joint vector of 
the manipulator, so a rotation free motion is obtained during the remaining part of 
the path. 

A problem arises when the joint vector increments for collision avoidance and 
joint range protection are added to Aq a to obtain the final joint vector increment 
Aq (equation 4.1). Since the joint vector increments for obstacle avoidance and 
joint range protection are computed in joint space, they will in general result in 
both a translation and a rotation in cartesian space. This is undesirable, since it 
will affect the orientation or position, that has been determined to be equal to the 
goal. 

To overcome this problem we add the two joint vector increments to obtain 
Aq^ as done in equation 4.2, and split the result into two orthogonal components 
Aq r o and Aq rx such that Aq r o is in the null space of the Jacobian J. 

Aq,. = Aqro + Aqnc (4.35) 


AqrO • Aqrx — 0 


JAq r o = 0 


(4.36) 

(4.37) 
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If we use A q r0 instead of A q r in equation 4.1, then the collision avoidance will 
not have any effect on the end effector position in cartesian space. Nevertheless, if 
the robot is redundant, the robot’s joint vector will still change and increase the 

distance to obstacles and joint range boundaries. 

As discussed earlier, we would like to stop either translational or rotational 

motion, once either has reached its goal. If we reduced Aq r to the null space of 
the full Jacobian, then collision avoidance would have no effect on any cartesian 
motion, neither translation nor rotation. But if we assume that the rotation was 
completed first, then there is no reason to inhibit effects of collision avoidance on the 
translational motion. However, any effect on the rotational motion must be avoided, 
since the correct orientation has been reached. This partial effect on cartesian 
motion can be obtained by reducing the joint vector to the null space of a part of 
the Jacobian matrix. As we have seen in equations 4.24 and 4.25, the rows of the 
Jacobian have a translational and a rotational part. In this sense we can split the 
complete Jacobian matrix into a translational and a rotational part: 

(4.38) 

Both J t and J r are 3 by n matrices, where n is the number of degrees of 
freedom of the manipulator. If we need to inhibit translational motion, we reduce 
Aq r to the null space of J t and if we need to inhibit rotation, we reduce it to J r ’s 

null space. 

There remains the question of how to reduce a joint vector to the null space 
of a Jacobian, complete or partial. The derivation follows: 



JAqr = J(Aqro + Aqrx) 
JAq r = JAqro+JAqj-x 


(4.39) 

(4.40) 
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JAcjro = 0 (4.41) 

=$> JAq r = JAqj-x (4.42) 

Here we can find the minimum norm solution for Aq^ as described in sub- 
section 4.2.6 below. Then 


Aqro - Aq r — Aq rx (4.43) 

provides the null space component of Aqr- 

4.2.6 A method for solving general linear systems 

Some of the methods discussed in the previous sections involve solving linear 
systems of the form 


AX = B (4.44) 

so it is worthwhile to show a method of finding the solution X. A is an n by 
m matrix, which leads to three different cases: 

n = m A is a square matrix; there is one solution if A is nonsingular. 

n > m The linear system is overdetermined; in general there is no solution, but we 
can find the least square error solution. 

n < m The linear system is underdetermined; let’s assume that in this case the un- 
known is a vector (x). There is an infinite number of solutions. We are 
interested in the solution that minimizes x T Qx where Q is a weight matrix. 
If Q = I, then the minimum norm solution is found. 

Let us first describe the case of A being a square matrix, since the other two 
cases are based on that case. 
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A Gaussian elimination algorithm is used, so the original system must first be 
transformed into an equivalent system A'X = B' in which A' is an upper triangular 
matrix, and which has the same solution X. The new system can then easily be 
solved by using back substitution. This transformation is done in n - 1 steps known 
as Householder transformations. [5] A Householder transformation is a matrix H 
that transforms a vector a into another vector b of equal norm: 

Ha = b with |a| = |b| (4-45) 

H can be found from a and b with reasonable computational effort: 

H = I - 2xx t with x = * _ k (4-46) 

It is now possible to obtain the upper triangular matrix A' by applying n - 1 
Householder transformations to parts of matrix A each one transforming a column 
of A into a column of an upper triangular matrix. We start with the leftmost column 
ai of A and find its norm KV Now we can find a Householder transformation H x 
that transforms a x into a vector with (n a ) x as the first element and all the other 
elements equal to zero, since the two vectors will clearly have the same norm. We 
obtain a new system of equations: 


HiAX = HiB 


(4.47) 


The new system matrix H X A has a particular structure. 


K)i 

d\2 • ■ • a ln 

0 


; 

a 2 

0 

_ 


HiA = 


(4.48) 
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This is clearly the first step towards an upper triangular matrix. We repeat 
this procedure for the submatrix A 2 and an equivalent submatrix of HiB, namely 
HiB with the first row omitted. After the second step, the second column will 
contain all zeroes except the first two elements. It can be seen that this procedure 
leads to an upper triangular matrix after n — 1 steps. 

The advantage of using Householder transformations as opposed to the simple 
Gaussian elimination procedure is that Householder transformation exhibit better 
numerical properties. The numerical behavior can be further improved by exchang- 
ing the first column of the current submatrix with the column having the largest 
norm. Then the elements on the diagonal of the resulting upper triangular matrix 
will be sorted; the element in the top left corner will be the largest, the element in 
the bottom right corner the smallest element in the diagonal. All elements on the 
diagonal will be greater than zero, since they are the norms of the columns before 
transformation. This fact provides a very convenient way of checking whether the 
matrix is singular or not. If the determinant of a matrix is zero, then the matrix 
itself is singular. The determinant of an upper triangular matrix is equal to the 
product of the diagonal elements. Since the elements on the diagonal are sorted by 
absolute value, we just have to check the last element on the diagonal. If it is equal 
to zero, the matrix is singular; if it is greater than zero, the matrix is nonsingular. 
Note that the exchanges performed on the columns of the system matrix must also 
be performed on the rows of X after back substitution. 

This covers the case of A being a square matrix. The other two cases can 
be reduced to the first case, as shown in the following. Let us begin with the case 
n > m, in which the system is overdetermined. In this case the least square error 
solution to the system can be found by solving the following system: 

a t ax = a t b 


( 4 . 49 ) 
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Finally, let us consider the case n < m and let’s assume that the unknown is 
the vector x. We are interested in the solution that minimizes x T Qx aubject to the 
constraint equation Ax = b. This is a minimization problem that can be solved by 
introducing a Lagrangian vector A: 

min ix T Qx + A T (b - Ax) (4.50) 

The partial derivatives with respect to x and A must equal zero: 


Qx — A t A = 0 (4-51) 

Ax-b = 0 (4-52) 

Premultiplying 4.51 with Q -1 leads to 

Q- 1 A T A = x (4.53) 

Both A and x are unknown in this equation, so we have n + m unknowns. 
We can however obtain an equation with only A as an unknown which reduces the 
number of unknowns to less than half, since A is an n by 1 vector and n < m. So 
let us premultiply 4.53 with A: 


AQ -1 A t A = Ax 
From equation 4.52 this leads to 


(4.54) 


AQ -1 A T A = b (4-55) 

The inverse of Q can be obtained very easily if Q is a diagonal matrix: The 
diagonal elements of Q -1 are the reciprocals of the diagonal elements of Q. A can 
be solved for as described above. The solution x can be found from equation 4.51: 
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CHAPTER 5 

COMBINING GLOBAL AND LOCAL PATH PLANNING METHODS 


5.1 Potential field methods and their problems 

Potential fields can be used effectively to produce safe, smooth paths around 
obstacles, but they have a serious drawback: If a path exists, but the potential field 
contains local minima, then there is no guarantee that the path will be found. In 
relatively simple two dimensional cases, it is often possible to choose the potential 
field such that it can be proven to contain no minima other than the global minimum 
in the goal. This task becomes very difficult when considering a strut modeled by 
a line segment moving in an environment of other line segments, and it becomes 
hopelessly complex when a reasonably accurate model of the manipulator is added. 

Thus it is not possible to rely on a potential field method alone. This chapter 
describes how a potential field approach can be embedded into a backtracking algo- 
rithm that calls the potential field method under various conditions, until a path is 
found. 

5.2 Different ways of performing a given task 

As an example of a task, consider the insertion of a strut into an existing 
structure. For the case that a strut is symmetrical, there would be two ways to 
complete this task. Figure 5.1 shows the two possibilities. The strut is drawn 
asymmetrically for clarity. 

In this example, the choice A is probably more likely to be successful than 
choice B, since the angle of rotation is smaller. This is of course not more than a 
heuristic statement and given certain joint angle configurations, it could very well 
turn out that B is successful and A runs out of joint range. But it is still preferable 
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Figure 5.1: Two ways of inserting a strut 

to try A first, since the probability for success is still higher and the resulting 
path also looks more natural. At this point we need an algorithm that finds the 
preferable orientation. Instead of comparing the angles of rotation involved in the 
two possibilities, which is somewhat difficult to find, we compare two much simpler 
expressions: 


D = |gi - Pi| + |g2 - Pal (5- 1 ) 

D is the sum of the distances between each end of the strut in its start and 
its goal position. We compare Da and Db , the distances for either orientation 
possibility; the smaller D value corresponds to the favorable orientation. 

Once we have decided on an orientation, there are two possible ways to perform 
the task. In subsection 4.2.1 of chapter 4 the rotation of the gripper from its current 
orientation to its goal orientation was discussed. We obtained the axis of rotation 
by finding the eigenvector of the rotation matrix, that corresponds to eigenvalue 1. 
It was also mentioned that this eigenvector describes the axis of rotation that allows 
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Figure 5.2: Choosing the correct sense of rotation 

the gripper to reach its goal orientation, but it provides no information about the 
sense of rotation. In fact, we can reach the goal orientation no matter what sense of 
rotation we choose, but the angles of rotation will in general be different, since the 
sum of the two angles is always 360°. Here we will of course first try the rotation 
involving the smaller angle of rotation, but even this is not always the better choice. 
Consider Figure 5.2. 

The indicated sense of rotation is clearly the only possible choice in this par- 
ticular case, even though the angle of rotation is greater than 180°. 

This yields four possible ways of performing the same task of moving a strut 
from a given start to a given goal position. 

5.3 Splitting into subtasks using graph search 

If none of the four cases leads to a successful completion of the potential field 
method, then it is necessary to split the task into subtasks. An example is shown 
in Figure 5.3. 
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Figure 5.3: Splitting a task into two subtasks 

The figure shows only one of many ways the intermediate step could be chosen. 
The idea is to provide a set of possible intermediate steps and use it to build an 
undirected graph. The nodes of this graph are the intermediate steps plus the start 
and the goal position of the strut. The selection of suitable intermediate steps is 
crucial for the success of the algorithm; it is described in subsection 5.3.2. After 
the nodes are created, edges are inserted such that every node is connected to every 
other node. In the example of Figure 5.3, we would obtain a graph with three nodes 
and three edges connecting them. 

5.3.1 Application of the A* algorithm 

Now we use the A* algorithm to find the shortest path leading from the start 
to the goal node. [4] We introduce the notion of distance by assigning a weight to 
every edge. This weight is the sum of the distance between the centers of the two 
node struts p and g attached to the edge and a measure for the degree of rotation 


involved: 
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w{p,g) 


i Pl+P2 _ 61+82 
1 2 2 

5 


+ C w (l — (Pp • Pg) ) 


with s = |pi — P 2 I (5-2) 


Consider Figure 4.1 for the meaning of the vectors; p p and p g are unit vectors, 
5 is the strut’s length and C« is a positive constant that determines the weight 
assigned to rotation differences. If this value is large, more weight is assigned to 
rotation. A rotation of 90° and a translation along the distance of sC* have the same 
weight. Note that identical orientation and exactly opposite orientation are both 
leading to zero orientation weight, so struts are again assumed to be symmetrical. 

The A* algorithm requires an estimate h of the cost of going from the node 
it currently works on (p) to the goal node. This estimate must be lower than the 
actual cost of reaching the goal in order to guarantee that A* will find the optimal 
path. In this application h can be defined using the weight function defined above: 


h(p) = w{p, goal) ( 5 - 3 ) 

The first path found by the A* algorithm is very obvious: It consists of the one 
edge connecting start and goal node directly. This result is passed to the potential 
field stage, so before trying any intermediate steps, the algorithm will always give 
the potential field method a chance to find a path by itself. If it fails (after trying 
all four possibilities described in section 5.2), this edge is eliminated from the graph 
and A* is applied to the rest of the graph. The result will now include at least two 
edges; they are both passed to the potential field stage. As soon as one of the edges 
leads to a failure, this edge is removed from the graph and a new path is generated 
by A*. This procedure is repeated until the potential field method is successful on 
all edges of a path or until no path connecting the start and the goal node is left. 
In the latter case, all resources are exhausted and the program reports a failure to 
the supervisor level. 



Figure 5.4: Intermediate steps placed around a tetrahedron 

5.3.2 Selecting subgoals for the truss structure problem 

The generation of subgoals is a very important step in every global path plan- 
ner. There should not be too many intermediate steps, since the number of paths 
grows exponentially with the number of nodes in the graph. The intermediate steps 
should also be placed in positions where they can help to guide the potential field 
algorithm around difficult areas. So examples of good intermediate steps are corners 
of obstacles. 

The approach taken for the specific case of truss structures is simple: Since 
the tetrahedron is the unit of all larger structures, all tetrahedra are extracted from 
the list of struts in the environment. When five of the six struts of a tetrahedron 
are present, then the structure is recognized and treated as a tetrahedron. The 
extraction algorithm is presented in subsection 5.3.3 below. Then an intermediate 
step is placed along every edge of every tetrahedron, as shown in Figure 5.4 for one 
tetrahedron. 

This approach could be improved in the two following ways: 
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• Generate an approximate model of the robot’s workspace and omit intermedi- 
ate steps outside the workspace. If an inverse kinematics procedure is available, 
apply it on every intermediate step and reject it, if the inverse kinematics has 
no solution. 

• Find the convex hull of the existing structure and accept only intermediate 
steps outside the convex hull. 

While this approach is described here for specific structural models, analogous meth- 
ods may be used for many common polyhedral models of objects. 

5.3.3 Extraction of tetrahedra 

The input to the program is a list of struts given by their positions in cartesian 
space. There is no a priori information about existence and location of tetrahedra, 
and they must be extracted from the list of struts. 

A tetrahedron as a three dimensional object consists of four different geometric 

primitives: 

• four vertices (O-dimensional) 

• six edges (1-dimensional) 

• four faces (2-dimensional) 

• the tetrahedron itself (3-dimensional) 

The edges represent the struts in the tetrahedron. The vertices are found by travers- 
ing the edge list and checking the two ends of each edge. If there is already a vertex 
in the vertex list at the position of a given end, then we create a pointer from the 
edge to that vertex, otherwise we create a new vertex at that position and a pointer 
from the edge to the new vertex. Every new vertex is put into the vertex list. 
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The extraction of the faces is more complicated. Again we traverse the list of 
edges and do the following for each edge e: It turns out that in a structure based on 
tetrahedra as shown in Figure 1.1 every vertex can have at most 12 edges attached 
to it. So for every end of e we traverse the edge list and store every edge that 
points to the same vertex this end of e points to. This procedure produces two lists 
with a maximum of twelve edges in it — the neighbors of the two ends of e. Then 
we traverse one of the lists of neighbors and for every neighbor rii we traverse the 
other list of neighbors and call the elements in this list n 2 . If a pair (r^ n 2 ) has 

a common node, then we detected a face bounded by the edges e, n\ and n 2 . Now 
we just have to make sure that this face was not detected before. This can be done 
by computing the center of gravity of the new face and comparing it to the centers 
of gravity of the other faces. If there is no face at the same position, we add it to 
the face list. Note that in the structures we are considering, there are never two 
different faces with the same center of gravity. If Pi, P 2 and P 3 are the vectors 
representing the three vertices of a face, then the center of gravity can be obtained 
as follows: 


Pc = Pl + P 3 2 + g (5.4) 

Having the list of faces, the extraction of the tetrahedra is not difficult. We 
traverse the list of faces and for every face /i, we traverse the list of faces again 
and call the current face f 2 . Now we compute the distance d between the centers of 
gravity of fi and f 2 . If the faces are part of a common tetrahedron, then d = s/3 if s 
is the length of the struts. As mentioned before, we treat two faces of a tetrahedron 
as a full tetrahedron, since at most one strut is missing once we detected two faces. 
We again have to make sure that this tetrahedron was not detected earlier. This 
can again be done by comparing the centers of gravity. If the four vertices in a 
tetrahedron are represented by the vectors pi, P 2 , P3 and P4i then the center of 
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gravity is: 

_ Pi + P2 + P3 + P4 (5 5) 

Pc - 4 

This yields a list of tetrahedra found in the structure. All comparisons of 
vectors involved in the process must allow for a considerable error, since the position 
inputs are noisy real world data. In order to create the intermediate steps, we 
compute the vector pointing from the tetrahedron’s center of gravity to the center 
of the edge under consideration. This vector must be multiplied with a constant 
that will specify the distance between the edges and the corresponding intermediate 
steps. The ends of the step can be obtained by adding this scaled vector to the ends 
of the edge in the tetrahedron. 


CHAPTER 6 

SOFTWARE DOCUMENTATION 

The path planning algorithm described in the previous chapters has been imple- 
mented in C language under the UNIX operating system. It has been developed on 
the SUN 3/60 and SPARC workstations in CIRSSE. The graphical user interface is 
based on SUNcore, SUN Microsystems’ graphics library, which restricts this imple- 
mentation to SUN platforms. However, all other parts of the program are portable 
to other platforms. 

6.1 Concepts 

The code is divided into 15 modules each consisting of a code file (f ilename . c) 
and a header file (f ilename. h). The code files contain the public and private pro- 
cedures, the private constants and type definitions and the private global variables. 
The header files contain public constants, type definitions and procedure declara- 
tions. After the declarations, every header file also contains a documentation of 
the module with an overview and a detailed description of all public procedures. 
Thus the header files should provide enough information to enable a user to use the 
module effectively. 

6.1.1 Global variables 

It was decided that no global variables should be visible from outside a mod- 
ule. All data transfer between the modules is carried out through procedure calls. 
This makes module communication slightly slower, but changes in the module are 
generally easier to accomplish without affecting the module interface. 
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6.1.2 Data types 

A module typically contains one or more data types on which the procedures 
of the module operate. A typical data type as found in the module s header file may 
be defined as follows: 

typedef struct complex 

{ 

float real ; 
float imag; 

> COMPLEX; 

This could be the data type of a module implementing operations on complex 
numbers. As a convention, all instances of data types printed in all uppercase 
(COMPLEX) are allocated in the heap memory. These data types all have a procedure 
to make a new instance and a procedure to kill an instance associated to them. 

COMPLEX *New_Complex () 

void Kill.Complex (c) 

COMPLEX *c; 

As a convention, other modules using the complex number module only use 
the pointer returned by the creator procedure and never directly access data inside 
the data structure. So instructions as 

c->imag = 2.0; 
x * c->real; 

are not allowed outside the complex number module. A procedure that adds 
the two complex numbers 4 + 5i and 2 — Si would look as follows: 
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#include "complex .h" 


void Addition_Example () 

COMPLEX *a, *b, *c; 

a = New. Complex () ; 
b = New. Complex C) ; 
c = New.Complex () ; 
Set.Complex (a, 4.0, 5.0); 
Set.Complex (b, 2.0, -8.0); 
Add.Complex (a, b, c) ; 
printf ("Result: '/.f + %f*i\n" 
Real (c) , Imag <c) ) ; 
Kill.Complex (a) ; 
Kill.Complex (b) ; 
Kill.Complex (c) ; 

> 


/* make three new instances */ 


/* set a to 4+5i */ 
/* set b to 2-8i */ 
/* add a and b, put result to c */ 

/* print result */ 
/* remove instances from heap */ 


the variables a, b and c are never accessed in 


It can be seen that the fields of 
the code, even though the C compiler would allow this, since the definition of data 
type COMPLEX resides in the header file which is included in the user module. If other 
modules actually accessed the fields directly, then a change in data type COMPLEX 


would necessitate changes in all modules that make use of complex numbers. With 


this convention, changes in the data type COMPLEX don’t affect other modules. 


6.1.3 Module hierarchy 

There are two ways in which module A can make use of module B. A can use 
B’s procedures and declare variables of a type defined in B. For this type of link, 
A would include the header file of B into its source file. This is the more common 
way of using another module. If A contains a public procedure that returns a value 
of a type defined in B, then A must include the header file of B into its own header 
file, in order to have B’s type definitions available in A’s header file. This leads to a 
problem, if a third module makes use of both modules A and B, and thus includes 
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both header files into its code file. Since header file B is included into header file A, 
it will be included twice and all types and procedures will be redeclared. In order 
to avoid this, the code in every header file is inclosed in the following structure: 

#ifndef MODULE_LABEL 
#define MDDULE.LABEL 

definitions, declarations ... 

#endif 

It ensures that every header file is compiled only once. Figure 6.1 shows the 
complete module hierarchy. Solid lines stand for normal links and dotted lines 
represent an inclusion of a header file into another header file. Arrows point from 
the including to the included module. 

6.1.4 Make 

The whole program can be compiled and linked using the UNIX utility make. It 
checks the last update time of every file and decides which files need to be recompiled, 
make needs information about the dependencies among the files of the program. This 
information is stored in the makefile. Code files and header files are included in this 
dependency tree, so a change in a header file will lead to an automatic recompilation 
of all files that include this header file. The dependency tree reflects the hierarchy 
shown in Figure 6.1. 

6.2 Documentation of modules 

This section includes a description of each module of the program. The same 
descriptions appear in the overview sections of the module’s header files. 

The modules are ordered by increasing level in the hierarchy. This way the 
reader becomes familiar with the prerequisites as they are needed to understand the 
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modules in higher levels. 

6.2.1 The “global” module 

This module is included by every other module of the program. It includes 
the two standard header files stdio.h and math.h, defines the constant II and the 
boolean data type and provides standard procedures for displaying error conditions 
on the screen. It also contains a customized version of the atan2 function. 

6.2.2 The “spec” module 

This module provides information about the machine the program is running 
on. This includes availability of a graphics screen and whether or not the screen has 
color capability. 

6.2.3 The “1st” module 

The list module provides a way of putting any kind of data into a sequential list. 
A list consists of a main list data structure (LIST) and a number of list elements 
(LIST_EL) representing the data elements. These list elements are dynamically 
allocated, so no information about the list’s length is needed. This is the main 
advantage of using this module over using a simple array. 

The LIST.EL datatype contains a pointer next pointing to the next LIST.EL 
and a pointer data that points to the listed data element, thus a simple forward 
chained list is implemented. However, this chaining mechanism is totally hidden in 
the module, so the fact that the user’s data types must be stored in a list has no 
impact on their internal structure. The list structure is shown in figure 6.2. 

Lists are built by adding elements to the end or the beginning of the list. The 
most common way of reading a list is by sequential access using the procedures 


Get.First and Get_Next. 
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user module 


list module 


Figure 6.2: List structure 

The module also provides random access, but since this procedure must go 
through the chain of list elements, the access is slow for long lists. To improve 
random access performance, the module allows the creation of an index array. This 
index contains the pointers to the data elements in a contiguous block of memory 
so that quick random access becomes possible. However it must be noted that every 
change in the list caused by adding or deleting an element automatically destroys 
the index, so indexed list access is only possible if the list is not changed after the 
index is created. 

6.2.4 The “stack” module 

The stack module provides a way of organizing any data in stacks (LIFO - 
buffers). Each data entry is represented by an instance of STACK_EL. This data 
structure holds a pointer to the user’s data and the chaining information. Access to 
the stack is accomplished by the procedures Push and Pop. The procedure Read.Top 
allows reading the latest entry on the stack without removing it. 
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6.2.5 The “vector” module 

The vector module provides three data types: 

• A column vector with 3 elements 

• A 3x3 matrix 

• A 4x4 homogeneous matrix with 4th row omitted (assumed [0 0 0 1]) 

The elements of the vector are floats, the columns of the 3x3 matrix are vectors 
and the homogeneous matrix is comprised of a matrix and a vector as the 4th column. 
All three data types are typically used for normal variable declarations; no instances 

of these types are allocated in memory. 

The module also provides a set of useful operations on vectors and matrices. 
For instance the distance computation between two line segments as described in 
section 3 is realized in this module. 

6.2.6 The “alg” module 

This module provides a set of operations on m by n matrices (linear algebra). 
The basic data structure is a variable (VAR) which may be a matrix, a vector or a 
scalar. A variable automatically adapts its size to a matrix that is assigned to it, so 
the user does not need to know the dimensions of the result of an operation ahead 

of time. 

There are some element oriented functions that require the specification of row 
and column values (typically parameters r and c). As a convention, the first row 
or column is number 0, so the element in the top left corner has row and column 
indices (0, 0). Names of functions returning a value of type VAR begin with a capital 
V (example: Vadd). All functions returning a BOOLEAN return TRUE, if they are 
completed successfully and FALSE if a problem is encountered. 
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The algorithm for solving general linear systems as described in subsection 
4.2.6 is implemented in this module as function Vsolve. 

The following example program will assign values to A, B and C, will evaluate 
the expression A + B * C, assign the result to X and print it on the screen. 



1 


2 0 


0 

A = 

3 

B = 

i 3 

C = 

_ -1 _ 


•include "alg.h" 
main () 

{ 

VAR *A, *B , *C, *X; 


Init_Var () ; 

A = New_Var 0 ; 

B * New_Var () ; 

C = Nev.Var () ; 

D = New.Var (); 

Put (Vuser (2, 1, 1.0, 

3.0), A); 

Put (Vuser (2, 2, 2.0, 0.0, 

1.0, 3.0), B); 
Put (Vuser (2, 1, 0.0, 

-1.0), C); 

Put (Vadd (A, Vmult (B, C)), X) ; 
Print _Var (X) ; 

Kill.Var (A); 

Kill.Var (B) ; 

Kill.Var (C) ; 

Kill.Var (X); 

Exit.Var () ; 

> 


/* initialize module */ 

/* make the variables */ 


/* makes A a 2x1 variable [1] */ 
/* [3] */ 
/* makes B a 2x2 matrix [2 0] */ 
/* [1 3] */ 
/* makes C a 2x1 variable [ 0] */ 

h [- 1 ] */ 

/* eval A+B*C, put result in X */ 
/* print X on the screen */ 
/* free space */ 


/* exit module */ 


6.2.7 The “graph” module 

The graph module provides a means to organize any kind of data in a directed 
or undirected graph. The data structure consists of a main structure (GRAPH) and 
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the two structural elements G.NODE for the nodes (or vertices) and G_EDGE for the 
edges of the graph. 

The GRAPH data structure contains a list of the graph’s nodes. Every node 
in turn has a list of adjacent edges. If the graph is directed, then the node’s list 
contains only adjacent edges that are pointing away from that node. Every edge 
has two pointers to the two nodes it is connected to. These two pointers are called 
nodel and node2. If the graph is directed, the edges are always pointing from 
nodel to node2. Both the edges and the nodes have a pointer to a data structure 
in the user’s module. In an example of a graph representing cities and connecting 
roads the nodes would contain a pointer to CITY and the edges a pointer to ROAD. 
Only the user’s data structures are used for communication between the modules 
so the internal structures G_N0DE and G_EDGE are invisible for the user. These data 
structures are shown on Figure 6.3. 

An edge in a graph always has an associated weight that represents the cost 
of traversing the edge. In directed graphs, the edges cannot be traversed in the 
wrong direction, it is however possible to define two edges between the same two 
nodes having opposite directions and different weight values. This weight value is 
not passed to the edge at the time the graph is being established, but the user must 
provide a weight function that returns the weight of any edge to the graph module. 
This way the graph module can query the weights whenever they are needed and no 
unnecessary weights are computed. If the computation of the weights is complicated, 
then this feature can save a considerable amount of computing time. Once the weight 
is computed, it is stored in the edge structure, so the computation is done only once 
per edge. This implies that an edge’s weight cannot change during the lifetime of 
the graph. 

The module offers procedures for building, changing and deleting graphs and 
the graph search algorithm A*. 
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Data structure : 


Graph : 






Figure 6.3: Data structures in the graph module 
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6.2.8 The “parser” module 

The parser module provides a convenient way of reading information from an 
input text file. The text in the file must conform to the following syntax: 


expression. 

par_list 

keyword 

parameter 

string 

char 


{expression} 
keyword [par_list] 

’('{parameter ’,’} parameter ’)’ 

string 

string 

{char} char 

’A’..’Z’ 1 'a’..’z' I 'O’. .'9' I 

'+’ | | I I 


In this syntax description, S is the start symbol, lowercase words are nonter- 
minal symbols and characters in single quotes are terminal symbols. An expression 
in braces {} can be repeated any number of times (including zero times) and an 
expression in square brackets [] is optional. If there are a number of expressions 
separated by bars | then either expression is legal at this point. 

Examples for legal commands are: 

ADD (5, 6, 7, -11.5) 

Exit_Program 
save&quit (foo.c) 

The parser module will first read a user specified source file, parse it according 
to above syntax, store the data in a list of expressions and return this list to the 
user. The order in the list corresponds to the order in which the expressions are 
encountered in the source file. If there are syntax errors, they will be printed on 
the screen. The module offers a variety of interface procedures that enables the 
user to read the data in a convenient manner. Expressions can be read from the 
list sequentially as it is normally done with lists. Lists can also be scanned for the 
next occurrence of an expression with a particular keyword. An expression is a 
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data type (EXP) that also has some procedures associated to it. The user can read 
an expression’s keyword string, the number of parameters in the expression and a 
particular parameter string given by its number in the parameter list. Finally there 
are utility procedures that convert a parameter string to a real or an integer number. 
This is necessary since all parameters are handled as arbitrary strings. 

6.2.9 The “model” module 

This module provides a geometric primitive which is useful for modeling of 
solids. The primitive is described by two points pi and p2 and a radius r. 

It is the object obtained by moving a sphere of radius r on a straight line from 
point pi to point P2 (a cylinder with spherical caps). 

Procedures are provided to read and change the model’s parameters and to 
compute the minimum distance between two swept sphere models. 

6.2.10 The “graphics” module 

This module provides an interface to a subset of SUNcore that allows line 
and character drawing in three dimensional space. Colors are used if the monitor 
allows and if black and white mode is not explicitly selected. After initialization, 
a three dimensional coordinate system is displayed. There are procedures to create 
segments - an entity that holds a number of primitives - and others to create lines 
and characters at arbitrary locations in three dimensional space. 

Other procedures allow the user to insert primitives into a segment and delete 
them from segments. 

Yet another procedure allows the user to rotate the current picture around 
the vertical and the horizontal screen axis by moving the mouse horizontally or 
vertically respectively. This mode stops in the current orientation when the middle 
mouse button is pressed. 
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The reason for using segments is the segment concept of SUNcore. The SUN- 
core segments do not provide any way of deleting single primitives stored in them, 
so the whole segment must be deleted and reconstructed in order to delete one prim- 
itive. This module automatically deletes and reconstructs the SUNcore segments as 
needed, but this process is visible on the screen, especially on slow machines. The 
segment concept allows the user to split the picture into parts, in order to avoid 
reconstruction of the whole picture when a single primitive is deleted. 

6.2.11 The “env” module 

“env” stands for environment, so this module holds the data about all items 
that belong neither to the robot nor to its payload. At initialization, the module 
reads the locations of the struts from the input file (see subsection 6.3.2 for strut 
position descriptions) and from the CIRSSE interface. Then it automatically tries 
to extract tetrahedra and places intermediate steps around the tetrahedra it found. 
This process is described in subsection 5.3.3. 

Procedures are provided to get models of the struts and intermediate steps 
currently in the environment and to add and remove struts. Whenever a strut is 
added or removed, all intermediate steps are deleted, tetrahedra are extracted and 
the intermediate steps reestablished based on the new set of tetrahedra. 

6.2.12 The “robot” module 

This module contains a model of a single chain robot with an arbitrary number 
of links. The description of the robot’s kinematics, model geometry, joint ranges 
and so forth are stored in the file robot. def, so that the files robot. c and robot .h 
can be applied for any single chain robot without changes. The robot s kinemat- 
ics are described using Modified Denavit Hartenberg parameters [27]. The module 
maintains a set of transformation matrices that represent the transformation from 
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each link to world coordinates. They are derived from the modified Denavit Harten- 
berg parameters and the current joint vector and are updated each time the robot 
changes its joint vector. The module also maintains a swept sphere model of each 
reasonably large link and a picture comprised of a set of lines for each link. The 
model and the picture are not automatically updated when the joint vector changes, 
since this process is time consuming and not always necessary. 

The module provides three procedures to alter the robot’s state. The robot’s 
joint vector can be set and a part can be added to or removed from the gripper. 

The various readout procedures supply information about the current position 
of the link models, the type of a particular link (revolute or prismatic), origin and 
axis of the joints, current value and range of each joint and whether the robot is 
carrying a payload or not. 

6.2.13 The “lpath” module 

The path planning algorithm using potential fields as described in section 4.2 
is implemented in this module. The user must specify the initial joint vector and 
the desired goal position in cartesian space and the module will return a list of joint 
vectors that describe a path leading there. If this is not possible, it returns a failure. 

6.2.14 The “gpath” module 

The global path planning algorithm as described in chapter 5 is implemented 
in this module. It establishes a list of joint vectors describing a path that leads from 
the current joint vector to a goal position defined in cartesian space. It may call the 
local path planner several times on the whole task or on part of it. 


59 


6.2.15 The “main” module 

The main module is responsible for initializing all other modules that require 
initialization. Then it checks to see if there is a command sequence in the input file. 
If it finds a START instruction, it takes the commands from the input file, otherwise 
it calls a CIRSSE interface procedure to read commands. This procedure is the main 
means of communication between this program and the higher level coordination 
program. Through it, commands are received and paths are returned. 

The set of available commands is described below for both input file and 
CIRSSE interface. 

6.3 Interface to CIRSSE 

6.3.1 CIRSSE interface procedures 

The communication of this program with other modules of the CIRSSE soft- 
ware is handled by dedicated interface procedures. They are marked with the string 
####, so they can be located easily using a text editor or the UNIX command grep. 
They all have extensive comments and an example implementation to show their 
purpose, but most of them are not actually written, since they depend on how the 
communication between the programs is implemented. 

In the following, a list of the avaliable interface procedures is given: 

• ‘'main” : initialize communication 

This procedure is called before any other interface procedure and can be used 

to establish the communication channels to other programs. 

• u main”: command interface 

The program receives commands and their parameters through this procedure. 

The following commands can be sent: 


- MOVE: initiate a path planning process 
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— GRASP: grasp a strut from the environment 

- UNGRASP: release a strut to a specified position 

- UNGRASP_FREE: release a strut where the robot put it 
— ADD .STRUT: add a strut to the environment 

- REMOVE.STRUT: remove a strut from the environment 

- JOINTS: specify new joint vector 

— VIEW: change orientation of screen display 

- QUIT: quit program 

• “main”: path output 

The joint vectors describing the path are sent through this procedure. It is 
called once after every successful MOVE command. 

• “main”: report failure 

If a MOVE command cannot be executed because no path can be found, this 
procedure is called to report the failure to the coordinator. 

• “robot”: initial joint vector 

This procedure is called once at initialization time to read the joint vector 
of the robot. Then the program assumes that the robot follows its path. If 
this is not the case, then the joint vector can be adjusted in the course of the 
program by sending a JOINTS command. 

• “env”: initial strut positions 

This procedure is called once at initialization time to read the positions of all 
struts in cartesian space. The program will keep track of position changes in 
the course of the program if they are caused by the robot. It can be notified 
of other changes in the course of the program by using the ADD.STRUT and 
REMOVE.STRUT commands. 
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• “env”: strut length 

The length of the struts in use is passed to the program through this procedure. 

• “env”: strut symmetry 

This procedure reads information about symmetry of the struts. A strut is 
considered symmetric in this context if it can be added to the structure upside 
down. The path planner will take advantage of these geometric properties. 

• “robot”: robot position 

The coordinate frame used to specify the initial strut positions is assumed to 
be the world coordinate system. If the robot’s zero frame does not coincide 
with the world frame, then the position and orientation of the robot’s frame 
with respect to the world frame can be specified here. If the 9 degrees of 
freedom robot of CIRSSE is used, this procedure is obsolete. However, if the 
6 degree of freedom PUMA robot is used, then the position of the PUMA’s 
base frame with respect to the lab’s world frame must be specified here. 

• “robot”: robot definition 

This is the point where the file robot. def is included into the robot module. 
All information about a particular robot is stored in this file. 

• “robot”: calibration 

This procedure reads a set of Modified Denavit Hartenberg parameters ob- 
tained by some calibration procedure. 

• “graphics”: display coordinate transformation 

This procedure specifies the way the world frame is displayed on the screen. All 
three axes of the world frame can be displayed in six directions: up, down, left, 
right, pointing out of the screen, or pointing into the screen. This specifies the 
initial display which can be changed by user interaction when a VIEW command 
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is sent. 

6.3.2 Input file 

The input file is an auxiliary input source for the program that can be used for 
testing. When the program is embedded in CIRSSE, it will not need an input file, it 
will receive all input from the CIRSSE interface procedures. However, the two input 
sources are not exclusive, information can be passed to the program by using both 
CIRSSE interface procedures and an input file simultaneously. Usually the program 
will first check the file and then the CIRSSE interface for input. Some concepts 
are only available through the input file commands since they are not useful in an 
embedded system. For instance graphics display can only be activated through a 
command in the input file. 

There are two ways of describing the position of a strut in space. A strut can 
be located anywhere in space, for instance in its storage location. In this case a con- 
venient description is the position of the strut’s endpoints in cartesian space. This 
allows the description of every possible position in space. If we want to describe 
the position of a strut that is part of the structure being assembled, the endpoint 
coordinate description is not the most convenient one, since the endpoint coordi- 
nates depend on the overall position and orientation of the structure and on the 
strut’s position in the structure. It requires some calculation to find the endpoint 
coordinates. It is easier to specify the position and orientation of the structure once 
at initialization time and then describe a strut by giving the tetrahedron it is part of 
and the strut number in this tetrahedron, according to some numbering convention 
for the six struts of a tetrahedron. Consider Figure 6.4. 

The coordinate system (xo, yo, z_o) is the world coordinate system. Vector 
p describes the origin of the structure expressed in world coordinates. The vectors 
ri and T 2 specify the orientation of the structure with respect to world coordinates. 
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z 



Figure 6.4: Conventions for specifying a strut in a structure 
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The coordinate system (x, y, z) is used to specify a tetrahedron in the structure. 
The lengths of this system’s unit vectors e x , e y and e z are equal to the length of 
the tetrahedra’s edges. Then every triple of integer numbers describes the O point 
of one particular tetrahedron. For instance (0,2,2) describes the O point of the 
rightmost tetrahedron in the figure (O = p + 0e x + 2e y + 2e z ). This tetrahedron 
also shows the numbering convention used to specify a particular strut in a given 
tetrahedron. Thus it is possible to describe a strut fully by giving the four integer 
numbers (X, Y, Z, N) once strut length and structure position and orientation have 

been specified. 

In the following, the commands that can be used in input files are listed. 
The unit of length is meters, angles are given in degrees. The parameters xl, 
yl, zl, x2 , y2 , z2 will always denote a strut position defined by its endpoints 
whereas parameters X, Y, Z, N denote a strut position defined by its position in 
the structure. The first list contains the commands that supply “static” information 
to the system. They are read at initialization time; their order in the input file is 
unimportant, as long as they do not appear between START and QUIT. 

• STRUT (xl, yl, zl , x2, y2, z2) 

A strut defined by its endpoints is added to the environment. If necessary, 
the strut’s length is adjusted to the current strut length such that the center 
stays fixed. 

• STRUT (X, Y, Z, N) 

A strut defined by its position in the structure is added to the environment. 

• TETRA (X, Y, Z) 

All six struts of the specified tetrahedron are added to the environment at 
once. 


STRUTLENGTH (1) 
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Specifies the length of the struts. 

• STRUCTURE.LOC (px, py, pz, rlx, rly, rlz, r2x, r2y, r2z) 

Defines the location of the structure with respect to world coordinates. The 
parameters are vectors p, rj and r 2 from Figure 6.4. 

• GRAPHICS 

Causes the path planning process to be shown on the screen. 

• ZOOM (z) 

Parameter z causes the display to shrink or expand. 

• B&W 

Causes the graphics to be displayed in black and white even if the program is 
run on a machine with color screen. This is useful for making screendumps. 

• ROBOT (px, py, pz, rlx, rly, rlz, r2x, r2y, r2z) 

With this command the zero frame of the robot can be oriented arbitrarily 
with respect to the world coordinate system. Vector p denotes the origin 
of the robot’s zero frame with respect to world coordinates, ri denotes the 
orientation of the robot’s xq axis and denotes the orientation of its j/o axis. 

The second list shows commands that can be used in a command sequence. 
This sequence begins at the START command and ends at the QUIT command or at 
the end of the input file. The commands in between are executed according to their 
order in the file. 

• START 

Denotes the beginning of the command sequence. 

• MOVE (xl, yl, zl, x2 , y2, z2, dx, dy, dz) 

MOVE (X, Y, Z, N, dx, dy, dz) 
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The program plans a path that leads the payload strut (imaginary or real) to 
the indicated position without collision. The vector d specifies the direction 
of approach. The path will be planned such that the last few steps of the 
path will move the strut in this direction. This direction vector d is stored 
and will determine the direction of start for the next MOVE command. The 
end effector will start moving in direction — d and then turn towards the goal 
location. The very first MOVE command immediately starts moving towards 
the goal location. 

• GRASP (xl, yl, zl, x2, y2, z2) 

GRASP (X, Y, Z, N) 

The strut closest to the position specified is removed from the environment and 
grasped by the robot. The path planner will treat this strut as another link. 
Recomputation of the set of intermediate steps takes place in the environment 
module. 

• JOINTS (thetal , theta2, ... .thetaN) 

The robot jumps to the new pose specified by the joint vector. The number 
of parameters of this command is equal to the number of degrees of freedom 
of the robot. Future MOVE commands start off with this joint vector. Units 
are degrees for revolute joints and meters for prismatic joints. Note that 
the CIRSSE interface procedure described above requires radians for revolute 
joints! 

• UNGRASP (xl, yl, zl, x2, y2, z2) 

UNGRASP (X, Y, Z, N) 

The payload is released and added to the environment at the specified position. 
The user may want to move the strut to a position just short of the goal 
and leave the last few inches to a fine motion planner with visual feedback. 
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However the parameters of this command should be the precise goal position, 
so that the path planner has a correct model of the world. All three versions 
of UNGRASP also recompute the intermediate steps. 

• UNGRASP 

Unlike the other two UNGRASP commands, this command releases the strut 
and places it in the environment exactly at the position the robot brought it 
to. This position might be slightly different from the position given in the 
previous MOVE command due to tolerances in the path planning algorithm. 

• ADD.STRUT (xl, yl, zl, x2, y2, z2) 

ADD .STRUT (X, Y, Z, N) 

A strut is added to the environment at the indicated position. If the robot 
is the only tool used to manipulate the environment, then ADD.STRUT and 
REMOVE.STRUT below should be obsolete. However they enable the coordinator 
to inform this program of changes in the environment that occurred due to 
other reasons. 

• REMOVE.STRUT (xl, yl, zl, x2, y2, z2) 

REMOVE.STRUT (X, Y, Z, N) 

The strut closest to the indicated position is removed from the environment. 

• VIEW 

Execution of the command sequence is stopped and the user can use the mouse 
to change the orientation of the display. Execution resumes when the user 
presses the middle mouse button. This command has no effect if there is no 
GRAPHICS command in the file. 

• QUIT 

Denotes the end of the command sequence. 



CHAPTER 7 
RESULTS 

The performance of the proposed algorithm has been tested in various simulations, 

the results are presented in this chapter. 

A model of one of the robot arms at CIRSSE was used for all simulations. 
This robot is shown on Figure 7.1. It is a PUMA arm with six degrees of freedom, 
mounted on a platform with three degrees of freedom. Eight of the nine joints are 
revolute, the first joint is prismatic. The axes of all joints are shown in the figure. 
The model uses the correct kinematic parameters of the actual robot, however, the 
outlines are not to scale. 

Figure 7.2 shows the trivial path planning task of moving a strut from an 
initial to a goal position. The same scene is shown from different points of view in 
order to help visualization of the three dimensional model. The path is displayed 
by showing the position of the strut in the robot’s gripper at every iteration. In this 
simulation, the direction of start is along the z axis and the direction of approach is 
against the direction of the z axis. It can be seen that the program automatically 
generates an intermediate goal above the true goal position in order to approach the 
goal position in the specified direction. The seemingly unnecessary rotation during 
the first few iterations is due to the fact that the program starts rotation towards 
the goal orientation only after the distance of the end effector to its goal position 
has decreased under a critical value. Before this phase, the end effector orientation 
is dictated by collision avoidance and joint range requirements. In this case, the 
purpose of the rotation is to move the joints in the wrist towards the middle of their 
range. 

Figure 7.3 shows the insertion of the last strut into a tetrahedron. This task 
involves avoiding a collision with the partially completed tetrahedron. Note that 


68 



69 





71 


the strut is touching the tetrahedron in its goal position. This is possible because 
all repulsive forces are reduced as the goal position and orientation are approached. 
The repulsive forces are reduced to zero when 

• the strut’s orientation is correct. =; _ 

• the end effector’s distance to its goal position is small enough. 

• the end effector has reached a position such that a motion along the specified 
approach direction will lead to the goal position. 

The same task is performed on Figure 7.4, but the approach direction is chosen 
slightly different (compare the end effector orientation in Figure 7.3). This leads to 
a failure when the natural rotation as in Figure 7.3 is attempted; the joint range is 
violated. As a consequence, the algorithm performs the task with opposite sense of 
rotation, which leads to a large angle of rotation. This case is discussed in section 
5.2 and illustrated on Figure 5.2. 

Figure 7.5 shows the initial situation of a more difficult problem. The robot 
must move the strut to the marked goal position appoaching it in x direction. 


The result is shown on Figure 7.6. 



Figure 7.3: Completing a tetrahedron 
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Figure 7.5: Initial state of obstacle avoidance demonstration 







CHAPTER 8 

DISCUSSION AND CONCLUSIONS 


8.1 Computational Complexity • 

In this section, the complexity of the path planning algorithm is discussed. Due 
to their different structure, the local and the global stage are discussed separately. 

The local path planner performs an iteration in three steps: It first finds the 
attractive joint vector, which involves finding a solution of the Jacobian equation 
as the limiting algorithm. The Jacobian is a 6 by n matrix, where n is the number 
of degrees of freedom of the manipulator. The algorithm involves a transformation 
of this redundant system to a nonredundant system with a 6 by 6 system matrix, 
as shown in subsection 4.2.6. Thus the actual elimination algorithm takes a con- 
stant computing time per iteration, but the transformation of the nonlinear system 
involves some matrix multiplications; their complexity is 0{n). 

The second step in performing an iteration is the computation of the self 
collision avoidance vector q S elf- If equation 4.22 is considered and the computing 
time for fq is assumed to be constant, then the complexity of cjseif is 0(n 3 ). 

The third step, the computation of the joint vector q e nv avoiding collisions 
between links of the robot arm and obstacles in the environment has a complexity 
of 0(n 2 m), where m is the number of obstacles in the environment. This can be 
seen from equation 4.21. These three steps turned out to be the limiting parts of 
the algorithm for all cases that have been examined. 

Another step is the computation of the joint range vector Aq rr , which is linear 
in n. However, the computing time for this step is negligible for all n. 

The number of degrees of freedom of the manipulator n is not a critical pa- 
rameter since it will never be very large. A much more critical parameter for the 
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local path planner is the number of obstacles m, even though computing time grows 
only linearly with m. A possible solution to this problem is the implementation of a 
quick distance check between the manipulator and a given obstacle. If this distance 
was large enough, then this obstacle would be ignored in the collision avoidance 
procedure. Another method to reduce computing time is the exclusion of pairs of 
objects from the collision avoidance procedure that cannot possibly collide. This is 
typically the case for neighboring links and for collisions between very small links 
and obstacles. The small links can normally be included in the model represent- 
ing a neighboring link. This method has proven to be very effective and has been 
implemented in the current software. 

The global path planning stage is based on the A* algorithm, which is a very 
efficient graph search algorithm, but it still doesn’t solve the search problem in 
polynomial time. So care must be taken to keep the number of subgoals as low 
as possible. However, in all but very large cases the local path planning stage is 
computationally much more expensive than the graph search. 

If the local path planner fails to find a path between two given subgoals, 
then the corresponding edge will be deleted from the graph and the path planning 
algorithm will be restarted. If we assume that the number of subgoals is roughly 
proportional to the number of obstacles, then the number of edges in the graph 
grows quadratically with the number of obstacles, since every node in the graph 
is connected to every other node. This leads to a complexity of the backtracking 

algorithm of 0(m 2 ) in the number of obstacles. 

Another stage in the global planning context is the selection of subgoals. In 
the example of truss assembly, this is done by first extracting the tetrahedra and 
then defining the subgoals close to their edges. The complexity of the extraction 
algorithm is 0(m 2 ) with m being the number of struts. The computing time of this 
part proved to be negligible compared to other parts. 
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8.2 Generalization of geometric model 

The current implementation of the path planner uses the swept sphere cylin- 
drical model to represent objects. However, every geometric model can be used, 
since the path planning algorithm only requires the computation of a distance vec- 
tor between any two models. It is also required that the volume of the actual object 
is completely enclosed in the volume of the model representing it. 

8.3 Experiences with the potential field algorithm 

Potential field methods have the advantage that various soft constraints can 
be incorporated by adding up all components of the potential field. However, every 
component of the field has a constant factor associated to it and these constants 
must be carefully balanced in order to obtain good results. This is particularly 
important for the attractive field and the obstacle avoidance field. If the obstacle 
avoidance field is too strong compared to the attraction field, then a narrow but 
feasible path between two obstacles may not be found. If the obstacle avoidance 
field is chosen too weak, then collision avoidance cannot be guaranteed if the step 
size is large. 

The step size is defined by the norm of Aq, the incremental joint vector. If 
this step size is chosen small, then the local path planning process will be slow, 
since the computation time for one iteration is constant in a given environment. 
But if the step size is chosen too large, then problems occur when approaching an 
obstacle. One step might take the robot from a position with sufficient clearance 
to a position very close to the obstacle or even to a collision. A position very close 
to an obstacle leads to a strong repulsive force that drives the robot back out of 
the field’s immediate proximity, which leads to oscillations in the path, as shown in 
Figure 8.1. 

A related problem is the choice of the range of the repulsive force (equations 
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Figure 8.1: Step size selection 
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4.17 and 4.18). If the range is very small, then there will be a sudden increase 
in the repulsive force as the robot approaches an obstacle. Then oscillations may 
occur even if the step size is small. On the other hand, if the range is chosen too 
large, then the repulsive field of a cluster of several obstacles accumulates to an 
excessively strong field, which leads to a path with an unnecessarily large clearance 
to the cluster. The repulsive field range must be particularly carefully chosen, if 
any part of the path lies in the inside of a truss structure. In these cases it may 
be necessary to adapt the step size and the repulsive field range depending on the 
proximity of obstacles. This has not been tested in the current implementation. 

The last parameter discussed in this context is the function that keeps the 
joints within their boundaries (see Figure 4.6). If this function permits the joints 
to run very close to their boundaries and the step size is large, then the joints may 
run out of range. However, if the function keeps the joints far off their boundaries, 
then the workspace of the robot is restricted unnecessarily. 

8.4 Configurations and singularities 

Another component to the potential field could be a field that prevents the 
robot from moving close to singularities. The potential field would be a function 
of p = det 2 (JJ T ). If p = 0 then the robot is in a singular position. The distance 
from singularities is related to the dexterity of the manipulator [14] in the sense 
that the robot is more dextrous if its pose is not close to any singular position. 
Another advantage of avoiding singular positions is that the Jacobian equation is 
guaranteed to have a solution. Apart from begin very difficult to implement in an 
efficient manner, singularity avoidance has another drawback: it prevents the robot 
from changing configuration. 

A nonredundant robot normally has several joint angles in which it reaches 
a given end effector position and orientation. Every joint angle corresponds to a 
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configuration of the robot. A redundant robot has in general infinitely many joint 
angles for a given end effector position and orientation, but these joint angles lie on a 
number of self motion manifolds in joint space that correspond to the configurations 
of the nonredundant robot. The robot can only change these configuration manifolds 
by passing through a singularity. Now assume the robot is in a given initial position 
that lies in one of the configuration manifolds. It may not be possible to reach a goal 
position within this manifold due to joint range restrictions. Then the only way to 
reach the goal is to change configuration manifold, so it is necessary to pass through 
a singularity. Our algorithm does not force the robot to explore all configuration 
manifolds as part of the search process, but it also does not actively prohibit the 
robot from changing manifold by driving it away from singularities. 

This concludes the discussion of the proposed path planning algorithm. 
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APPENDIX A 
Simulation input file 


{ Command sequence 
> 


start 


view 

{ first strut > 

move (1.79, -0.8, 1.0, 0.9, -0.8, 1.0, 0.0, 0.0, 
grasp (1.79, -0.8, 1.0, 0.9, -0.8, 1.0) 
move (0, 0, 0, 1, 0.0, 0.0, -1.0) 
ungrasp (0, 0, 0, 1) 

view 

{ second strut } 

move (1.79, -0.8, 0.9, 0.9, -0.8, 0.9, 0.0, 0.0, 
grasp (1.79, -0.8, 0.9, 0.9, -0.8, 0.9) 
move (0, 0, 0, 2, 0.0, 0.0, -1.0) 
ungrasp (0, 0, 0, 2) 

view 

{ third strut > 

move (1.79, -0.8, 0.8, 0.9, -0.8, 0.8, 0.0, 0.0, 
grasp (1.79, -0.8, 0.8, 0.9, -0.8, 0.8) 
move (0, 0, 0, 3, 0.0, 0.0, -1.0) 
ungrasp (0, 0, 0, 3) 

view 

{ fourth strut ) 

move (1.79, -0.8, 0.7, 0.9, -0.8, 0.7, 0.0, 0.0, 
grasp (1.79, -0.8, 0.7, 0.9, -0.8, 0.7) 
move (0, 0, 0, 4, 1.0, 0.0, 0.0) 
ungrasp (0, 0, 0, 4) 

view 


- 1 . 0 ) 


- 1 . 0 ) 


- 1 . 0 ) 


- 1 . 0 ) 


{ fifth strut > 

move (1.79, -0.8, 0.6, 0.9, -O.B, 0.6, 0.0, 0.0, -1.0) 
grasp (1.79, -0.8, 0.6, 0.9, -0.8, 0.6) 
move (0, 0, 0, 5, 0.0, 1.0, -1.0) 
ungrasp (0, 0, 0, 5) 

view 

{ sixth strut > 

move (1.79, -0.8, 0.5, 0.9, -0.8, O.S, 0.0, 0.0, -1.0) 
grasp (1.79, -0.8, 0.5, 0.9, -0.8, O.S) 
move (0, 0, 0, 6, 1.0, 0.0, -1.0) 
ungrasp (0, 0, 0, 6) 

view 


quit 


{ Environment info 

> 


graphics 

structure.loc (0.8, 
strut (1.79, -0.8, 
strut (1.79, -0.8, 
strut (1.79, -0.8, 
strut (1.79, -0.8, 
strut (1.79, -0.8, 
strut (1.79, -0.8, 
strutlength (0.89) 
zoom (2.0) 


-0.1, 1. 

0, 10, 0.0 

1.0, 0.9 

-0.8, 1.0) 

0.9, 0.9 

-0.8, 0.9) 

0.8, 0.9 

-0.8, 0.8) 

0.7, 0.9, 

-0.8, 0.7) 

0.6, 0.9 

-0.8, 0.6) 

0.5, 0.9 

-0.8, O.S) 


0 . 0 , 


0 . 0 , 1 . 0 , 


0 . 0 ) 
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APPENDIX B 
Listings of header files 


Module alg (linear algebra): 


Data types 


typedef struct var 

i " ' ” 

float *•; 
int r , c ; 
int size; 

> VAR; 

A variable (matrix, vector or scalar). rows and ’c’ columns, ’size* is r*c, 
so the number of elements. »e’ is a pointer to a linear array of ’size’ floats. 
The order is left to right, ten top to bottom. 


Procedure description 


void Init.Vars () 

This procedure initializes the ‘'ALG'* module and must be called before doing 
anything else. 

void Exit.Vars () 

Can be called in order to free all memory space used by the module. Io function 
should be called after ’Exit.Vars’. 

VAR *Iew.Var () 


Assigns a pointer to an initialized matrix to a pointer variable. Every pointer 
variable must be initialized this way before using it. 

After initialization the variable is empty (a 0x0 matrix). 

B00LEAI Put (a, b) 

VAR *a; 

VAR *b; 


Puts the value of ’a * which can be a variable or an expression, into T b’. 
This function must be used for every assignment operation! 

VAR *V0 (r, c) 
int r; 
int c ; 


Returns an r*c matrix with all elements equal to zero. 


VAR ♦VOnes (r, c) 
int r; 
int c; 


Returns an r*c matrix with all elements equal to one. 
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VAR *VI (din) 
int dim; 


Returns a dim*dim unit matrix. 

VAR *Vuser Cr, c, vl, v2, v3, ...) 
int r ; 
int c ; 

1 r*c ’ floats ; 

Returns an r*c matrix with user defined contents. The floats in the parameter 
list ara fillad in tha matrix from laft to right and from top to bottom. 

Tha usar must supply r*c floats in tha paramatar list. 

Example: 

Vuser (2, 3, 1.0, 2.0, 3.0, [123] 

5.0, 4.0, 3.0); creates [5 4 3 ] 

VAR eVscmult (v, s) 

VAR *v ; 
float s; 


Returns ’v’ multiplied elementwise with ’s’. 

Example : 

[ 1 23] multiplied with 2.0 is [2 46]. 

float Vector_Iorm (v) 

VAR *v; 


Returns the norm (length) of variable v. ’v’ must be a vector, i. e. ’v’ must 
have either one row or one column. 

VAR *Vadd (a, b) 

VAR *a; 

VAR *b ; 


Returns the elementwise addition of variables ’a* and *b’. ’a’ and ’b must 
have the same dimensions. 

VAR *Vsub (a, b) 

VAR «a; 

VAR *b ; 


Returns the elementwise subtraction of variables ’a* and ’b’. ’a’ and ’b’ must 

have the same dimensions. 

VAR *Vmult (a, b) 

VAR *a ; 

VAR *b ; 


Returns the product of variables ’a’ and ’b’. The number of rows of ’a’ must 
equal the number of columns of ’b’. 

VAR *Vtranspose (v) 

VAR *v ; 


Returns the transpose of variable v. 

VAR *Vsolve (a, b, q, success) 
VAR *a; 

VAR *b ; 

VAR *q; 

BGOLEAI *success; 


Returns the solution of the linear system a*x*b. ’a’ and ’b» must have the 
same number of rows. If ’a’ is square, Wsolve’ returns the solution, if 
one exists (if ’a’ is nonsingular). If ’a’ has more rows than columns 4 


(system is overdetermined) , than ’Vsolve’ returns the least square 
approximation. If ’a’ has lass rows than columns (system is underdetermined) , 
then the x minimizing x’Qx (» denotes the transpose) is returned. Q is the 
IIVERSE of a diagonal matrix with the elements of vector ’q’ on the diagonal, 
thus V and *x’ have the same dimension. If ’q’ is IULL, then Q*I 
(unity matrix) is assumed and thus the minimum norm solution is returned. 

If a solution was found, ’success’ is set to TRUE, otherwise to FALSE. 

VAR *Vinv (a) 

VAR *a; 


Returns the inverse of variable ’a’, ’a’ must be squared. 

int Ib.Cols (v) 

VAR *v; 


Returns the number of columns of variable ’v’. 

int Ib.Rows (v) 

VAR *v; 


Returns the number of rows of variable ’v’. 

B00LEAI Fill.Var (v, r, c, num, vl , v2 , v3, ...) 
VAR *v; 
int r; 
int c ; 
int num; 

’num’ floats; 


This procedure is used to fill part of matrix W’ with user defined values. 
It starts filling at the element at row ’r’ and column ’c» and fills up 
from left to right and from top to bottom. It writes ’num’ values into 
the matrix. It is the user’s responsibility to supply ’num’ floats after 
the ’num’ parameter. If the matrix would overflow over the bottom right 
corner, an error occurs and no values are written at all- 

float Read.El (v , r, c) 

VAR *v ; 
int r; 
int c ; 


Returns the element at row ’r’ and column ’c’ in variable ’v’. 

B00LEAI Writa.El (v , r, c, val) 

VAR *v; 
int r; 
int c; 
float val; 


Writes ’val’ to the element at row ’r’ and column ’c’ in variable ’v’. 

VAR *Vcut (sre, r_src , c_src, r.size, c.size) 

VAR *src ; 
int r.src; 
int c_src; 
int r_size; 
int c.size; 

Returns a piece of variable ’src’. The top left corner of this piece is the 
element at row ’r.src’ and column ’c_src’ in variable ’src’. The piece 
has ’resize’ rows and ’c.size’ columns. An error occurs, if the specified 
piece is not part of matrix ’src’. 

BOGLEAI Paste (src, dest, r.dest , eldest) 

VAR *src; 
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VAR *dest; 
int r.dest; 
int eldest; 


Pastes variable ’sre’ into variable ’dest’. ’arc's top left corner goes 
to roe ’r.dest’ and column ’c.dest’ in variable >dest * . An error occurs 
if there is not enough room in ’dest’ to complete the operation. 

B00LEAI Snap. Rows (v, rl, r2) 

VAR *v; 
int rl; 
int r2 ; 


Rows ’rl and >r2’ in variable ’v» are exchanged. 

B00LEAI Swap.Cols (v, cl t c2) 

VAR *v; 
int cl; 
int c2; 


Columns ’cl and ’c2’ in variable *v* are exchanged. 

B00LEAI Print.Var (v) 

VAR *v; 


Variable »v’ is printed to the screen. ’Print.Var’ doesn’t care about the 
screen size, so large matrices may be hard to read. 

void Kill.Var (v) 

VAR *v] 

The Memory space of variable *v* is freed. Pointer ’v’ is invalid after 
’Kill.Var’ . 
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Module env (environment): 


Procedure description 


void Init.Env (source) 
LIST esource; 


Reeds the locations of all struts that are present in the environment at 
initialization time and the length of the struts in use- Both input file and 
CIRSSE interface are read. Then tetrahedra are extracted and interswdiate 
steps are generated. 

float Get .Strut .Length () 


Returns the stmt length. 
B00LEAI Symnetric () 


Returns TRUE if the stmts are symmetric in the sense that the endpoints can 
be exchanged for assembly. If this is not the case, FALSE is returned. 

B00LEAI Cet.Tetra.Pos (xt , yt, zt, nb, pi, p2) 
int xt; 
int yt; 
int zt ; 
int nb; 

Vector *pi ; 

Vector *p2; 

Transforms a strut position given in tetrahedron coordinates to cartesian 
coordinates of its endpoints pi and p2 . (xt , yt , zt) denote a tetrahedron 
in the structure and ’nb’ denotes the number of the strut in this tetrahedron 

( 1 . . 6 ) . 

MODEL *Get .First. St rut. Model (Ip) 

LIST.EL **lp ; 

MODEL *Get.Iext. St rut .Model (lp) 

LIST.EL **lp ; 

MODEL *Get .First. Inter. Step.Model (lp) 

LIST.EL **lp; 

MODEL *Get_Iext .Inter. St ep.Nodel (lp) 

LIST.EL **lp ; 


These procedures are used to read the list of strut models and intermediate 
step models and are equivalent to the standard list readout procedures 
described in the list module. 

B00LEAI Add.Strut (pl t p2> 

Vector pi ; 

Vector p2; 

Adds a strut to the environment. Its endpoints are at ’pi* and ’p2’. 
Intermediate steps are deleted, tetrahedra reextracted and intermediate steps 
recomputed . 

B00LEA1 Remove.Strut (pi, p2, rad) 

Vector pi; 

Vector p2; 
float *rad; 


Q ■'X- 
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Removes the strut closest to an imaginary atrut aith andpoints at 'pi' and 
>p2> Tha cloaaat strut is the strut ahose center is closest to the center 
of the imaginary strut betaeen >pl’ and ’p2>. This strut’s radius is returned 
in ’rad’. FALSE is returned if there aas no strut in the environment. 
Intermediate steps are deleted, tetrahedra reextracted and intermediate steps 
recomputed . 

MODEL •Get_Closest_Strut_Hodel (pi, p2 , dmin) 

Vector pi; 

Vector p2; 
float •dmin; 


Returns the model of the strut from strutlist closest to the position 
defined by ’dl ’ and ’d2’. The distance is returned in ’dmin’. If there 
is no strut in the strutlist, ’dmin’ takes a negative value, otheraise 
, dmin l is the distance between the centers of the struts. 
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Module global : 


Constants 


•define PI 3.141592654 


Data types 


•define B00LEAI int 
•define TRUE 1 
•define FALSE 0 


Procedure description 


void Warning (procname, msg) 
char *procname; 
char *msg; 

Prints the calling procedure 1 * name (’procname’) and a earning message (’msg’) 
and returns. 

void Error (procname , msg) 
char *procna»e ; 
char ♦msg; 

Prints the calling procedure’s name (’procname’) and an error message (’msg’) 
and returns. 

void Fatal (procname, msg) 
char eprocname; 
char *msg; 

Prints the calling procedure’s name (’procname’) and an error message (’msg’> 
and exits the program eith exit code 1 . 

void Fatal.Halloc (procname) 
char *procname; 

Prints the calling procedure’s name (’procname’) and the standard error 
message “memory allocation failed" and exits the program. 

This procedure is provided for convenience since every memory allocation 
must be checked for failure. 

It also returns with exit code 1. 

float Atan2 (x, y) 
float x, y; 


Like the built in math function atan2, but Atan2 (0.0, 0.0) * 0.0 
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Module gpath (global path planner applying graph search): 


Procedure description 


void Init_Path () 


Bust b. callad once before calling 


*Find_Path 1 for initialization. 


LIST *Find_Path (path, pi, p2, dir) 
LIST *path ; 

Vector pi; 

Vector p2 ; 

Vector dir; 


£ SkTSSJTL’K £tP!\£ elements'are joint v.ctor, (typo VAR) 
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Module graph (graph theory): 


Data types 


typedef struct graph 

LIST enodelist; 
float (height) (); 
BOOLEAV directed; 


> GRAPH; 


The data structure fur a graph, ’nod.li.f is a list sxth nod« of tha 
eraph The data type of the nodes is ’G.IODE’ (see graph. c> Weight is 
I pointer to a function that return, th. -eight of an edge. Th. graph is 
directed if ’directed’ is TRUE, undirected other-ise. 


■ 


Procedure description 


GRAPH elee.Graph (Weight, directed) 
float (eWeight) (); 

BOOLEAI directed; 

^Zlatas a nes graph data structure. The user must provide the function ’Weight- 

L ...,,.. .< - .... «• -• '< - - 1 - i “ 

follows : 

float Weight (nodel , node2 , edge) 
char *nodel , node2, edge; 

If the graph is directed, the module expects th. -eight of the “‘S* 8 ° in * 

•nodel’ to ’node2’. If parameter ’directed’ is TRUE, then a directed graph is 

created . 

void Connect (graph, node! , node2, edge) 

GRAPH * graph ; 
char *nodel; 
char *node2; 
char *edge; 


’nodel’ and -node2’ are connected by ’edge’, iny graph structure can be build 
by lust using this one procedure. If one of the nodes has been used in a 
previous call of ’Connect’, then the ne« edge is added to it. oth.r»ise a 
nes node is created automatically. In a directed graph an edge pointing from 
’nodel’ to ’node2 * is created. 

void Connect. All (graph, nodelist, Get.Edge) 

GRAPH egraph; 

LIST enodelist; 
char •(•Get.Edge) (); 


This procedure is useful for creating graphs in -hich every node is connected 
to every other node, ’nod.list’ contains the nodes of the graph and Get.Edge 
is a user provided procedure that is declared as follows, 
char *Get_Edge (nodel, node2) 

char *nodel, *node2; ... j 

This function must return the data associated to th. edge “* d 

> nod«2 ’ . This can be a IULL pointer which means that this edge doesn t hav* 
an eqival.nt data structure in the user’s module. In fact, the 
parameter can be a IULL pointer, too. This is th. case sh.n the edga. in the 
graph module generally don’t have an equivalent data structure in the user s 
module. In a directed graph every pair of nod., sill receive t-o edges 
pointing in opposite directions. 
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void Connect. All.Cond (graph, nodelist, Get.Edge, Condition) 
GRAPH * graph; 

LIST *nodelist; 
char ♦(♦Get.Edge) (); 

B00LEAI (♦Condition) (); 


This procedure works like ’Connect. Cond’ , the only difference is the 
additional parameter ’Condition’ which must be declared as follows. 

BODLEAI Condition (nodel , node2) 
char vnode 1, *node2 ; 

Before creation of an edge ’Connect.All.Cond ’ will call this function. If it 
returns TRUE, the edge is created, otherwise it isn’t. This feature is useful 
to set up visibility graphs: ’Condition’ must return TRUE if ’nodel’ is visible 
from ’node2’ and FALSE otherwise. 

B00LEAI Disconnect (graph, nodel, node2, edge) 

GRAPH * graph ; 
char *nodel; 
char *node2; 
char ♦edge ; 


This procedure is used to remove a single edge from the graph, ’edge’ between 
’nodel’ and ’node2’ is removed. TRUE is returned if this edge existed, FALSE 
otherwise . 

void Disconnect. All (graph) 

GRAPH *graph ; 

This procedure removes all edges from the graph. The nodes remain in the 
graph? 

LIST *A_Star (graph, Estimate, start , goal, edge.path) 

GRAPH * graph ; 
float (*Estimate) () ; 
char ♦start; 
char ♦goal ; 

LIST **edge_path ; 

The A-Star algorithm tries to find the optimal path from ’start’ to ’goal’. 
Optimal means minimal sum of edge weights along the path. 

’Estimate’ is a pointer to a user provided function: 

float Estimate (node) 
char *node ; 

It must return an estimate of the cost to go from ’node’ to ’goal’. If 
this estimate is always lower than the actual cost, A-Star will find the 
optimal path. 

If a path exists, A-Star will find it and return a list of the nodes it 
passed. In parameter ’edge.path’ it returns a list of the edges it went 
through. The two lists have the same length. The first edge is the edge 
between the first and the second node, so the l ast en try in the edge list 
is always a IULL. If no path exists, ITJLL is returned. 

void Kill.Graph (graph) 

GRAPH * graph ; 


Deletes the nodes, edges and the graph data structure making ’graph’ 
invalid. The user’s data for the nodes and edges are of course left intact. 
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Module graphics: 


Constants 


♦define 

BLACK 

0 

♦define 

WHITE 

1 

♦define 

RED 

2 

♦define 

YELLOV 

3 

♦define 

BLUE 

4 

♦define 

GREEV 

5 

♦define 

GRAY 

6 


Data types 


typedef struct lino 

< 

Vector pi ; 

Vactor p2; 
int color; 

int styla ; 

> LIVE; 

LIVE raprasants a lina on the screan. ’pi’ and *p2* ara tha endpoints, 
’color 5 can take ona of tha values definad above and ’style* is either 
SOLID, DASHED, or DOTTED. 


typedef struct character 

{ 

char c ; 

Vactor pos; 
int color; 

> CHAR; 


CHAR represents a character *c’ on the screen, 
’color’ is one of the colors defined above. 


’pos 9 indicates its position. 


typedef struct sag 

{ 

LIST *linelist; 

LIST •charlist; 

int segnum; 

BOOLEAV active; 

> SEG; 

SEG represents a segment that contains a number of lines and characters. The 
lines (LIVE) are stored in ’linelist’ and the characters (CHAR) in ’charlist’ 
’segnum’ is the SUIcore segment number, ’ ac tive* is TRUE if the segment is 
nonempty and must be included in updates and rotations. 


Procedure description 


void Init.Graphics (source) 
LIST •source; 


Initializes SUIcore in tha currant window and displays a coordinate system. 
Parameter ’source* is a list of expressions from the parser. If expression 
’BAH’ is found, the graphics are displayed in black and white, even on a 
color screen. This can be useful for screendumps. 



If & ’ZOOK (x) ’ expression is found, then the display is enlarged or 
shrunk according to x. 

B0OLEAI Graphics. Active () 

^tu™r«ra"if"soicorrhM been successfully initialize, FALSE oth.mis. 
void Exit .Graphics () 

Should be called before exiting the program. 


void Spin.Graphics () 

Enables” the”user” t o rotate the picture around the vertical or the horizontal 
acreen axis by moving the mouse horizontally or vertically respectively 
This procedure ends in the current orientation when the user presses the 
middle mouse button. 


SEG *Iew. Segment (> 


Returns a now segment. 

void Kill.Sogmont (seg) 

SEG *seg; 

Kills segment ’sag’. 

void Update. Segment (seg) 

SEG *seg; 

Redraws” segment” ’seg ’ . This is needed when there are changes in certain lines 
or characters in the segment that are not yet reflected on the screen. 

void Update.All. Segments 0 

Redraws all segments at once. 

void lo. Update () 

After this procedure is called, the screen is not updated when primitives are 
inserted in or deleted from a segment. This is useful .hen deleting many 
primitives at once to avoid repeated reconstruction of the segment, 
updating is turned back on by calling >Update.Segment • on any segment. 

LIKE *Iew.Line (color, stylo) 
int color; 
int stylo ; 

Returas”a”ne.”line”.ith”giveii”color and style (SOLID, DOTTED. DASHED). 

void Kill.Lino (1) 

LIKE *1; 


Kills lino »1» 

void Sot.Lino.Pos (1, pi, p2) 
LIKE *1 ; 

Voctor pi ; 

Voctor p2; 


Changos lino '1 1 s position. 

void Got.Line.Pos (1, pi, p2) 
LIKE *1; 

Voctor pi ; 

Vector p2; 


Returns lineal’s position. 

void Insert.Line (seg, 1) 

SEG *seg; 

LI1E *1; 

insert s'lina *1> into s.gm.nt ’sag- and display, it imm.diat.ly, if th.r. 
gas no previous ’lo.Update’. 

BOOLEAI Delete. Lin® (seg, 1) 

SEG *seg; 

LIVE *1; 

D«i«tas"line~'i" from s.gm.nt ’sag’ and r.fl.cts ths chang. i-sadiataly. if 
there was no previous ’^.Update*. 

CHAR *Iew_Char () 


Returns a new character. 

void Kill. Char (c) 
CHAR c; 


Kills character ’c’. 

void Change.Char () 

CHAR *c ; 

Vector pos; 
int color; 
char ch ; 

Changes position, color and letter of character ’c’. 

void Insert.Char () 

SEG *seg; 

CHAR *c ; 

iiisarts'eharacter -C into ..g».nt -sag’ and displays it im.adiat.ly, if thara 
was no previous ’lo.Update’. 

BOOLEAI Delete. Char (seg, c) 

SEG *seg; 

CHAR *c ; 

Dolatss”character >c- from sagmant -sag- and r.fl.cts th. chang. iss.adi.taly, 
if there was no previous ’lo.Update’ . 


Module Ipath (local path planning with potential fields): 


Procedure description 


void Init.Local.Path () 

Hult'be called before the first call of ’Local.Path.Plan’. 


void Jormalize.A.Vector (pi, p2, dir) 
Vector pi; 

Vector p2 ; 

Vector *dir ; 


Hakes' vector ’dir ’ length 1 and orthogonal to the line defined by’pl’ 
’p2’. ’dir’ eill remain in the plane defined by the line t roug p 
»p2 J and the line along the old ’dir’. 


B00LEAI Valid.Strut (pi, p2) 

Vector pi; 

Vector p2 ; 

Returns TRUE if ’pi’ f p2’ a P osition * or « intermediate step 


B00LEAI Local.Path.Plan (q, pi, p2, d, path, pict, sag, rot .normal) 


VAR 

Vector 

Vector 

Vector 

LIST 

LIST 

SEG 


•q; 

pi; 

p2; 

d; 

•path ; 
•pict ; 

•seg; 


B00LEAI rot.normal; 


Plans a path using a potential field method. The initial joint vector q 
is assumed and the path Bill lead the (real or imaginary) payload strut to 
endpoint positions ’pi’ and ’p2’. The goal Bill be approached in direction 
>d’ The path Bill be returned in list ’path’ ehich Bill contain a joint 
vector (VAR) for each step. List ’pict’ Bill contain the lines to display 
the path and segment ’seg’ Bill be used. If ’rot.normal’ is TRUE, then the 
angle less than 180 deg Bill be used to rotate the gripper * ts “ tar * 

to its goal orientation, ehich is normally better. If it is FALSE, the other 
sense of rotation Bill be used. Bhich involves an angle of rotation of more 
than 180 deg. 


void lew.Start.Dir (dir) 
Vector dir; 


Must be called before ’Local.Path.Plan’ if the path must leave the start 
position in a particular direction. This direction is AGAIIST the vector 
’dir’, so ’dir’ is normally the approach vector of the robot’s gripper in 
start position. 



Module list: 


Data typas 


typedef struct list.element 

i 

struct list.element *next ; 
char *data; 

> LIST.EL; 

El«a«nts of typo LIST.EL fora tho chain of list olo»ont». >noxt’ points to tho 
noxt olomont in tho list, ’data’ points to tho usor data roprosontod by this 
list element . 

typedef struct list 

{ 

struct list.element efirst, *last; 
int length; 

char eeindex; 

> LIST; 

LIST is the main list data structure, ’first* and ’last* point to the first 
and the last element in the LIST.EL chain, ’length* stores the number of 
elements currently in the list, ’index’ has a pointer to an array of user 
data pointers that alio* fast random list access. If an index doesn’t exist, 
’index’ is IULL. 


Procedure description 


LIST *Iew.List 0 

Creates a new list (allocates and initializes a LIST data structure) and 
returns a pointer to it. 

B00LEAI Insert (1st, data) 

LIST *lst ; 
char »data; 

BOOLEAI Insert. As.First (1st, data) 

LIST *lst; 
char *data; 

’Insert’ and ’Insert. As.First ’ are the t*o procedures to build a list. ’Insert 
adds the element ’data’ at the end, ’ Insert .As.First ’ at the beginning of the 
list. Existing indeces are destroyed by both procedures. 

BOOLEAI Delete (1st, data) 

LIST *lst; - 1 -A, - - 

char *data; 

Deletes element ’data’ from the list ’1st’. Returns TRUE if ’data’ was found 
in the list, FALSE otherwise. An existing index is destroyed. 

BOOLEAI Is.In.List (1st, data) 

LIST *lst ; 
char *data; 

■•turns TRUE if ’ data * is found in ’1st’, FALSE othsrsis*. 
char »G«t. First (1st, current) 



LIST *lst; 
LIST.EL **current ; 


char eGet.Iext (current) 

LIST.EL **current; 

MJet.First’ and ’Get.Iext’ allou sequential access to the list. * procedure 
using these functions typically looks as follows: 

void Sequent ial. Ac cess.Example (1st) 

LIST *lst; 

LIST.EL *lp; 

DATA.ITEH *data; 


data - (DATA.ITEM e)Get .First (1st, tip); 
while (lp) 

Process. Data.Item (data) ; 

data » (DATA. ITEM *)Get.Iext (tip); 

> 

> 


In this example the list holds elements of type DATA.ITEM. Since both 
’Get First’ and ’Get.Iext’ return pointers to type char, a type cast is 
necessary in most cases. The pointer variable ’lp’ points to the current 
list element. It is initialized to the first list element by Get.First 
and updated to the next element by ’Get.Iext’. When the end of the list 
is reached, ’lp’ is assigned TOLL, so loop control can be done using lp 
If a program contains nested loops, it is important to declare a separate 
element pointer variable for every loop that goes through » 

■ote that ’lp’ does I0T point to the data element of type DATA .ITEM, but to 
the list element of type LIST.EL that represents this data element. 


char *Get_Ith (1st, n) 
LIST *lst; 
int n ; 


This function is used for random access. If an index exists, the n-th element 
is returned very quickly, otherwise the function steps through the list 
sequentially and thus takes a little longer if ’n T is large. 
n*0 returns the first element. If ’n’ is too large, 


IULL is returned . 


char *Get.This (current) 

LIST.EL **current; 

retums'the data alomant raprasantad by tha list alamant that ’currant’ points 
to. ’current * is left unchanged. 


char *Get.Last (1st) 
LIST *lst ; 


Returns the last data element of list ’lst’. 

void Build. Index (1st) 

LIST *lst; 

Craatas an array of pointars to tha data alaments in tha list. Onca this indax 
axists, random accassas using function ’Oat. 1th’ bacoma much fastar Any 
function that changes tha list aill destroy the index automatically. 

B00LEAI Append (1st, lst2> 

LIST *lst; 

LIST *lst2 ; 

Appends tha elements of ’lst2’ to ’1st’. The list elements are duplicated in 



this process, so changing ’lst2' after ’Append’ has no effect on 1st 
TRUE is returned if ’Append’ uas successful, FALSE othersise. 

An existing index of ’1st’ is destroyed automatically! 

int List.Langth (lat) 

UST *lst; 


Returns the number of elements in the list f lst * 


void Erapty.List (1st) 

LIST *lst; 

Removes”all list elements from list ’1st’ leaving just the LIST data structure 
Any existing index is destroyed automatically. .... , 

lote that the data elements themselves are I0T affected in this process. 


void List_Apply_F (1st, Function) 

LIST *lst ; 

void (^Function) () ; 

Applies the user defined function ’Function’ to all elements of the list ’1st’ 
This function must be declared as follows: 

void User.Function (data) 
char *data; 

’data’ is the current data element in the list. 

void Kill.List (1st) 

LIST *lst; 

Removes all list elements and the LIST data structure itself, so ’1st’ is 
invalid after ’Kill.List’. Any existing index is of course deleted too. 
lote that the data elements themselves are I0T affected in this process! 


Module model (geometric modeling): 


Data types 


typedef struct model 

l 

Vector pi, p2; 
float r; 

> MODEL ; 


MODEL represents a seept sphere geometrical model. ItS „* point ’p2’ 

that a sphere of radius ’r’ seeeps ehen moving from point p P 


that a sphere 
on a straight line 


Procedure description 


MODEL *Iew.Model () 


Returns a nee instance of a model. 

void Set.Nodel.Parameters (m, pi, p2 , r) 

MODEL *w; 

Vector pi ; 

Vector p2; 
float r ; 


Changes all parameters of model ’m*. 

void Set.Model.Pos (m, pi, p2) 

MODEL 
Vector pi; 

Vector p2 ; 

Changes the endpoints of model *m 9 leaving its radius unaffected. 


void Set.Model.Radius (®» r) 

MODEL *m; 
float r; 

Changes model ’m’s radius leaving its endpoints unaffected. 


float Model. Distance (ml, m2, pi, p2) 
MODEL *ral ; 

MODEL *m2 ; 


Vector *pl ; 
Vector *p2 ; 


Computes the shortest distance beteeen models ’ml’ and 'm2’. It 
distance U d the tno closest points on the line segment, inside the soept 
sphere cylinder (parameters ’pl\ ’p2’). 


void Get. Model. Pos (m, pi, p2) 

MODEL *m; 

Vector *pl; 

Vector *p2; 

Returns the model endpoints in ’pi* ’P 2 *' 


void Get. Model. Radius (m, r) 
MODEL 
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Module parser. 


Data types 


typedef struct expression 

{ 

char ^keyword; 

LIST *par_list; 

> EXP; 


EW _ represents' express ions in an input fils. An expression a 

keyword and optionally a number of parameters in parentheses, separated 
by coimias . Examples; 

keyword 

keyword (parameter) 

keyword (parameter^ parameter2, parameter3, parameter 
■keyword’ points to the keyword string converted to uppercase. 
contains a list of strings that represent the parameters. They are also 
converted to uppercase . 


Procedure description 


LIST *Iew_ Source (fn&me) 
char fname □ ; 

This'procedure opens the text file -fname’ and parses it according to the 
module ’ s grammar If the file doesn’t exist or there are syntax errors, then 
a TOLL pointer is returned, otherwise a list of expressions as found in 

It' is 'possible* to set ’fname ’ to IULL. In this case, an empty expression list 
will be returned without error message. 


EXP *Get_First_Exp (source, lp, keyword) 
LIST esource; 

LIST.EL **lp ; 
char *keyword; 


EXP *Get_Iext_Exp (lp, keyword) 
LIST.EL **lp; 
char ^keyword; 


’Get First Exp’ and ’Get.Iext.Exp’ are very similar to ’Get.First’ and 
■Get'lexf'in the list module. In fact, if ’keyword’ is TOLL, they are 
equivalent. If ’keyword’ is a string, then ’Get.First Exp will re 
the first expression with this keyword and ’Get.Iext.Exp will retu 
next occurrence of an expression with this keyword from the current point 
in the list. The matching is case insensitive. The readout procedures are 
compatible to the list module in the sense that a part of the expression 
list can be read with the procedures in the list module and then a P ar *^ ar 
keyword can be searched from that point using ’Get.Iext.Exp . If no matchi g 
keyword is found, TOLL is returned and ’lp’ is set to TOLL. 


char *Get_Xeyword (exp) 

EXP *exp; 

Returns the uppercase keyword string of expression ’exp’. 


int Ib.Par (exp) 
EXP *exp; 
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Returns the number of parameters of expression exp 


char eGet.Par (exp, nb) 
EXP *exp; 
int nb; 


Returns uppercase 
parameter has ’nb’ 


parameter string number ’nb' in expression ’exp’. 
* 0. If 'nb' is too large, IULL is returned. 


The first 


B00LEAI Get.Float (par, v) 
char *par; 
float *v; 

Conv ring” P»^’ «•»« ’ v ’' Returns ™ E if succ «” ful 

FALSE otherwise . 


BOOLEAI Get.Int (par, v) 
char *par; 
int *v; 

Coii verts _ string""p»r"" int Mr*. Returns TRUE if successful 
FALSE otherwise. 


void Kill-Source (source) 

LIST esource; 

Sills”the”source"list ’source-. Kills ell expressions in it and the 
list itself. 





Module robot : 


Procedure description 


void Init.Robot (source) 
LIST esource; 


The robot is initialized and oriented according to the ROBOT 

input file. If no ROBOT command exists, the robot 

to the world coordinate system. (This is the case at CIRS 


command in the 
system is equal 


BOOLEAI Joints.In.Range (qq) 

VAR *qq; 

Ret urns _ raU^~ if" all _ joint values in >qq> are within range. FALSE otherwise 

BOOLEAI Set.Pos. Joints (new.q) 

VAR *nes.q; 

Sets’the'robot to the pose defined by joint vector 'new_q> and updates the 
transformation matrices. 


void Update. Model () 

Updates”the”swept "sphere models of the links to the current joint vector, 
void Updat e.Picture o 

Updat es _ thew ire" frame picture of the robot to the current joint vector. 

BOOLEAI I s.Revo lute. Joint (nb) 
int nb; 

Returns"TRUE”is"link””nb"”is I'revolut. joint, FALSE otherwise. The first 
link is link 1 . 


MODEL *Link. Model (nb) 
int nb; 


Returns'the model of link -nb- or TOLL if there is no swept sphere model of 
this link. If the model was not up to date, it is automatically updated. 

The first link is link 1. 


BOOLEAI Consider.For.Self. Collision (nbl, nb2) 
int nbl ; 
int nb2 ; 

Ret urns" TRUE”If”link""ubl'’”and”IInk , nb2 . CO uld collide and thus have to be 
considered in the collision avoidance procedure. 

The first link is link 1. 


BOOLEAI Consider.For.Arm.Collision (nb) 
int nb; 

Returns”TRUE _ if link -nb- could collide with an obstacle in the environment 
and thus has to be considered in the collision avoidance procedure. 

The first link is link 1. 


Vector Origin (nb) 
int nb; 

Ret urns' the" or igin'of the coordinate frame of link -nb*. Origin (0) returns 


the origin of the robot’s coordinate system. 


void Axis (nb, origin, dir) 
int nb; 

Vector eorigin; 

Vector *dir; 

Returns the axis of rotation (revolute) or the direction of motion (prismatic) 
of joint ’nb’. The origin is also returned. 

float Joint_Value (nb) 
int nb; 


Returns the current joint value of joint ’nb 5 . The first joint is number 1. 

void Joint.Range (nb, loner, upper) 
int nb; 
float *lower; 
float *upper; 

Returns the lowest and the highest possible value of joint ’nb’ . The first 
joint is number 1 . 

float Joint.tfeight (nb) 
int nb ; 

Returns the weight of joint ’nb’ used for solving the Jacobian equation. 

High values lead to high velocities of that joint. 

The first joint is number 1. 

void Grasp.Part (pi, p2, rad) 

Vector pi ; 

Vector p2; 
float rad ; 

Puts a swept sphere cylinder as described by ’pi’ and *p2’ and ’rad’ in the 
robot’s gripper. The positions of the endpoints will change in this process, 
but the length of the cylinder will be retained. 

void Ungrasp.Part (pi, p2) 

Vector *pl ; 

Vector *p2 ; 

Em pties the robot’s gripper and returns the last endpoint positions of the 
payload. 

B00LEAI Robot. Carrying () 

Returns TRUE if the robot is currently carrying a payload, FALSE otherwise, 

void Set .Pos.To. Pay load (m) 

MODEL *m ; : — — J ' 

Sets the endpoint positions of model ’m’ to the positions they would have, 
if the model was the payload of the robot. 

void Set.Pos.To.Gripper (m) 

MODEL *m; 

Sets the endpoint positions of model ’m’ to the positions they would have, 
if the model was the robot’s gripper. 


Module spec (functions providing CIRSSE/RAL specific information): 


Constants 


•define EARTH 
•define MARS 
♦define HERCURY 
♦define JUPITER 
•define SOL 
•define VEIUS 
♦define IEPTUIE 
•define H00I 


0xll0070el 

0x13004882 

0xl700c726 

0xl700cdd7 

0x21000411 

0x5100c045 

0x51006ee6 

0x51001639 


Procedure description 


B00LEAI Graphics.OK () 


Returns TRUE if the machine on ehich the program 
screen and suncore is available. 

BOOLEAI Color.OK () 


Returns TRUE if the machine on shich the program 


is running has a graphics 


is running has a color screen. 



Module stack : 


Data types 


typedef struct stack. el 

i 

struct stack.el *prev; 
char *data; 

> STACK .EL ; 


STACK.EL represents an entry of a stack, 
entry and ’data’ points to the user data 


’prev’ is a pointer to the previous 
represented by this stack element. 


Procedure description 


void lev. Stack (sp) 

STACK.EL ♦♦sp; . _ .. . 

Hust'b* called before using a stack to initialize stack pointer >sp>. ’sp> is 
declared as follows: 

STACK.EL *sp ; 

Initialization : 
lew. Stack (tsp) ; 

void Push (sp, data) 

STACK. EL +**p) 
char *data; 


Places Mata * on stack ’sp’ 

char *Pop (sp) 

STACK. EL ^sp; 

Returns the last data entry and removes it from the stack 

char *Read_Top (sp) 

StACK.EL **sp ; 


Returns the last data entry without changing the stack 


Module vector. 


Data types 


typedef struct 

{ 

float x, y, z; 

> Vector; 

Represents a 3x1 vector with elements *x\ *y’ , z 

typedef struct 

{ 

Vector vi , v2 , v3; 

> Matrix; 

Represents a 3x3 matrix with column vectors ’v2’ , and 


•v3>. 


typedef struct 

Matrix m; 
Vector v; 

> H. Matrix ; 


[0 0 0 1 ]. 


Procedure description 


Vector Vec (x, y, z) 
float x; 
float y; 
float z; 

Creates a vector with elements x, y, z and returns 


Matrix Mat (vl , v2, v3) 
Vector vl; 

Vector v2 ; 

Vector v3; 


Creates a matrix sith column vectors vl, v2 


v3 and returns it . 


H_Hatrix HJlat (ra, v) 
Matrix m; 

Vector v; 


Creates and returns a homogeneous matrix oith matrix 


and 4th column vector 


Vector Add (a, b) 

Vector a; 

Vector b; 

Returns the sum of vectors a and b. (elementuise) 


Vector Sub (a, b) 
Vector a; 
Vector b; 


Returns the difference a-b. (elementwise) 

Vector Mul (a, s) 

Vector a; 
float s; 


Multiplies the elements of a with s and returns the result. 

Vector Div (a, s) 

Vector a; 
float s; 


Divides the elements of a by s and returns the result . 

Vector leg (a) 

Vector a; 


Returns the elementwise negation of vector a. 
Vector lull.Vector () 


Returns the null vector [000]. 

float Dot.Prod (a, b) 

Vector a; 

Vector b; 


Returns the scalar or dot product of vectors a and b. 

Vector Cross. Prod (a, b) 

Vector a; 

Vector b; 


Returns the cross product of vectors a and b. 

Matrix Dyad.Prod (a, b) 

Vector a; 

Vector b; 


Returns the outer or dyadic product of vectors a and b. 

float Length (a) 

Vector a; 


Returns the length (absolute value) of vector a. 

Vector Scale (a, len) 

Vector a; 
float len; 

Changes the length of vector a to ’len’ and returns the result. 
Prints an error message if ’a’ is a null vector. 

Vector Scale. If .Longer (a, len, limit) 

Vector a; 
float len; 
float limit; 


If the length of vector a is longer than ’limit 5 then its length is changed 
to ’len’, otherwise it is returned unchanged. 

Vector formal (a, b) 

Vector a; 

Vector b; 
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Returns vector ’a’ projected on a plane normal to vector ’b’ . 

Vector Center (a, b) 

Vector a; 

Vector b; 

Returns a vector whose endpoint is in the center between the endpoints of the 
vectors a and b. 

BOOLEAI Parallel (a, b) 

Vector a; 

Vector b; 

Returns TRUE if vectors a and b are parallel, FALSE otherwise. 

float Distance. Point.Line (point, line.pl, line_p2, line. result) 

Vector point; 

Vector line.pl; 

Vector line_p2; 

Vector eline.p. result ; 


Returns the shortest distance between ’point’ and the line bounded by the 
points ’line.pl ’ and ’line_p2’. * line.result ’ will contain the point on the 
line which is closest to ’point’. 

Vector MxV.Prod (m, v) 

Matrix m; 

Vector v; 


Returns the product of matrix w and vector v. 

Vector HMxV.Prod (hm, v) 

H.Matrix hm; 

Vector v; 

Returns the product of the homogeneous matrix hm and vector v. The 4th element 
of ’v’ and the result are omitted and assumed to be 1. 

Matrix MxM.Prod (ml, m2) 

Matrix ml ; 

Matrix m2; 


Returns the product of matrices ml and m2. 

H.Matrix HMxHM.Prod (hml , hm2) 

H.Matrix hml; 

H.Matrix hm2 ; 

Returns the product of the homogeneous matrices hml and hm2. 

float Det (m) 

Matrix m; 


Returns the determinant of matrix ra. 

Matrix Transpose (ra) 

Matrix ra; 


Returns the transpose of matrix ra. 

Matrix Inv (ra, ok) 

Matrix m; 

BOOLEAI • ok; 

Returns the inverse of matrix m. If inversion was possible, ’ok’ is set to 
TRUE, otherwise to FALSE. 
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Matrix I.Hatrix O 


Returns the identity matrix [100 

0 10 
0 0 1 ] 

B00LEAI Plane_Line_Intersection (plane^p ,plane_dl ,plane_d2 , lineup ^line^d ( 

t^planel, t_plane2, t„line, distance) 

Vector plane.p; 

Vector plane_dl ; 

Vector plane_d2; 

Vector lina.p; 

Vector line.d; 
float *t_planel ; 
float *t_plane2; 
float et_line; 
float ^distance; 


Intersects the plane defined by location vector ’plane^p* and direction 
vectors ’plane.dl* and , plane.d2’ with the line defined by location vector 
’lineup’ and direction vector ’line.d’ . If this intersection is possible, 
TRUE is returned and the parameters of the intersection point for both 
the plane and the line are returned. The two equations for the intersection 
point are : 

ip * plane.p + t.planel • plane_dl + t^plane2 * plane_d2 
ip * lineup + t_line * line_d 

If intersection is not possible (line is parallel to the plane) then FALSE 
is returned and the distance between the plane and the line is returned in 
’distance ’ , 

float Distance_Line_Line (al , a2, bl , b2, a.result, b^result) 

Vector al ; 

Vector a2 ; 

Vector bl ; 

Vector b2 ; 

Vector *a_result ; 

Vector *b_ result ; 

Returns the distance between the line bounded by al and a2 and the line 
bounded by bl and b2. The points of closest distance on the lines are 
returned in ’ a_result’ and ’b.result’. 

Vector Random_Vector (len) 
float len; 


Returns a random vector of maximum length ’len’. 

void Print.Vector (a) 

Vector a; 


Prints vector a to the screen. 

void Print.Hatrix (m) 
Matrix m; 


Prints matrix m to the screen. 

void Print,H_Matrix (hm) 
H.Matrix hm; 


Prints the homogeneous matrix hm to the screen. 


